Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 255 | f9daq | 1 | #include "uSMC.h" |
| 2 | |||
| 3 | #ifdef uSMC_MAIN |
||
| 4 | //# include "uSMC_ui.h" |
||
| 5 | #endif /* uSMC_MAIN */ |
||
| 6 | |||
| 7 | #include <stdlib.h> |
||
| 8 | #include <stdio.h> |
||
| 9 | #include <string.h> |
||
| 10 | |||
| 11 | #ifndef _WINDOWS |
||
| 12 | # include "libusmc.h" |
||
| 13 | # include <getopt.h> |
||
| 14 | # define Delay(x) usleep((int)(x*1000000)) |
||
| 15 | #else |
||
| 16 | # include "USMCDLL.H" |
||
| 17 | # include <utility.h> |
||
| 18 | # include <ansi_c.h> |
||
| 19 | /* |
||
| 20 | # define ERR(s, c) if(opterr) fprintf(stderr,"%s %c\n",s,c); |
||
| 21 | int opterr = 1; |
||
| 22 | int optind = 1; |
||
| 23 | int optopt; |
||
| 24 | char *optarg; |
||
| 25 | |||
| 26 | int getopt(int argc, char **argv, char *opts){ |
||
| 27 | static int sp = 1; |
||
| 28 | int c; |
||
| 29 | char *cp; |
||
| 30 | |||
| 31 | if(sp == 1) |
||
| 32 | if(optind >= argc || |
||
| 33 | argv[optind][0] != '-' || argv[optind][1] == '\0') |
||
| 34 | return(EOF); |
||
| 35 | else if(strcmp(argv[optind], "--") == NULL) { |
||
| 36 | optind++; |
||
| 37 | return(EOF); |
||
| 38 | } |
||
| 39 | optopt = c = argv[optind][sp]; |
||
| 40 | if(c == ':' || (cp=strchr(opts, c)) == NULL) { |
||
| 41 | ERR(": illegal option -- ", c); |
||
| 42 | if(argv[optind][++sp] == '\0') { |
||
| 43 | optind++; |
||
| 44 | sp = 1; |
||
| 45 | } |
||
| 46 | return('?'); |
||
| 47 | } |
||
| 48 | if(*++cp == ':') { |
||
| 49 | if(argv[optind][sp+1] != '\0') |
||
| 50 | optarg = &argv[optind++][sp+1]; |
||
| 51 | else if(++optind >= argc) { |
||
| 52 | ERR(": option requires an argument -- ", c); |
||
| 53 | sp = 1; |
||
| 54 | return('?'); |
||
| 55 | } else |
||
| 56 | optarg = argv[optind++]; |
||
| 57 | sp = 1; |
||
| 58 | } else { |
||
| 59 | if(argv[optind][++sp] == '\0') { |
||
| 60 | sp = 1; |
||
| 61 | optind++; |
||
| 62 | } |
||
| 63 | optarg = NULL; |
||
| 64 | } |
||
| 65 | return(c); |
||
| 66 | } |
||
| 67 | */ |
||
| 68 | #endif /* _WINDOWS */ |
||
| 69 | |||
| 70 | // Function that prints information about device state to console |
||
| 71 | void PrintDState(USMC_State *State) { |
||
| 72 | printf( "The state is:\n" ); |
||
| 73 | printf( "- Current Position in microsteps - %d\n", State->CurPos ); |
||
| 74 | printf( "- Temperature - %.2f\xf8\x43\n", State->Temp ); |
||
| 75 | printf( "- Step Divisor - %d\n", State->SDivisor); |
||
| 76 | printf( "- Loft State - %s\n", State->Loft?"Indefinite":"Fixed" ); |
||
| 77 | printf( "- Power - %s\n", State->Power?(State->FullPower?"Full":"Half"):"Off" ); |
||
| 78 | if(State->RUN) |
||
| 79 | printf( "- Step Motor is Running in %s Direction %s\n", |
||
| 80 | State->CW_CCW?"CCW":"CW", ((State->SDivisor==1) && State->FullSpeed)?"at Full Speed":"" ); |
||
| 81 | else |
||
| 82 | printf( "- Step Motor is Not Running\n" ); |
||
| 83 | printf( "- Device %s\n", State->AReset?"is After Reset":"Position Already Set" ); |
||
| 84 | printf( "- Input Synchronization Logical Pin State - %s\n", State->SyncIN?"TRUE":"FALSE" ); |
||
| 85 | printf( "- Output Synchronization Logical Pin State - %s\n", State->SyncOUT?"TRUE":"FALSE" ); |
||
| 86 | printf( "- Rotary Transducer Logical Pin State - %s\n", State->RotTr?"TRUE":"FALSE" ); |
||
| 87 | printf( "- Rotary Transducer Error Flag - %s\n", State->RotTrErr?"Error":"Clear" ); |
||
| 88 | printf( "- Emergency Disable Button - %s\n", State->EmReset?"Pushed":"Unpushed" ); |
||
| 89 | printf( "- Trailer 1 Press State - %s\n", State->Trailer1?"Pushed":"Unpushed" ); |
||
| 90 | printf( "- Trailer 2 Press State - %s\n", State->Trailer2?"Pushed":"Unpushed" ); |
||
| 91 | if( State->Voltage == 0.0f ) |
||
| 92 | printf( "- Input Voltage - Low\n"); |
||
| 93 | else |
||
| 94 | printf( "- Input Voltage - %.1fV\n", State->Voltage); |
||
| 95 | } |
||
| 96 | |||
| 97 | // Function that prints information about device start parameters to console |
||
| 98 | void PrintDStartParameters(int DPos, float Speed, const USMC_StartParameters *SP ) { |
||
| 99 | printf( "Destination position - %d\n", DPos); |
||
| 100 | printf( "Speed - %.2ftacts/s\n", Speed ); |
||
| 101 | printf( "Steps Divisor - %d\n", SP->SDivisor ); |
||
| 102 | if(SP->SDivisor == 1) { |
||
| 103 | printf( "Slow start/stop mode - %s\n", SP->SlStart?"Enabled":"Disabled" ); |
||
| 104 | } else if(SP->LoftEn) { |
||
| 105 | printf( "Automatic backlash operation - Enabled\n" ); |
||
| 106 | printf( "Automatic backlash operation direction - %s\n", SP->DefDir?"CCW":"CW" ); |
||
| 107 | printf( "Force automatic backlash operation - %s\n", SP->ForceLoft?"TRUE":"FALSE" ); |
||
| 108 | } else { |
||
| 109 | printf( "Automatic backlash operation - Disabled\n" ); |
||
| 110 | } |
||
| 111 | if(SP->WSyncIN) |
||
| 112 | printf( "Controller will wait for input synchronization signal to start\n" ); |
||
| 113 | else |
||
| 114 | printf( "Input synchronization signal ignored \n" ); |
||
| 115 | printf( "Output synchronization counter will %sbe reset\n", SP->SyncOUTR?"":"not " ); |
||
| 116 | } |
||
| 117 | |||
| 118 | // Function that prints information about device parameters to console |
||
| 119 | void PrintDParameters(USMC_Parameters *Parameters) { |
||
| 120 | printf( "The parameters are:\n" ); |
||
| 121 | printf( "Full acceleration time - %.0f ms\n", (double) Parameters->AccelT ); |
||
| 122 | printf( "Full deceleration time - %.0f ms\n", (double) Parameters->DecelT ); |
||
| 123 | printf( "Power reduction timeout - %.0f ms\n", (double) Parameters->PTimeout ); |
||
| 124 | printf( "Button speedup timeout 1 - %.0f ms\n", (double) Parameters->BTimeout1 ); |
||
| 125 | printf( "Button speed after timeout 1 - %.2f steps/s\n", (double) Parameters->BTO1P ); |
||
| 126 | printf( "Button speedup timeout 2 - %.0f ms\n", (double) Parameters->BTimeout2 ); |
||
| 127 | printf( "Button speed after timeout 2 - %.2f steps/s\n", (double) Parameters->BTO2P ); |
||
| 128 | printf( "Button speedup timeout 3 - %.0f ms\n", (double) Parameters->BTimeout3 ); |
||
| 129 | printf( "Button speed after timeout 3 - %.2f steps/s\n", (double) Parameters->BTO3P ); |
||
| 130 | printf( "Button speedup timeout 4 - %.0f ms\n", (double) Parameters->BTimeout4 ); |
||
| 131 | printf( "Button speed after timeout 4 - %.2f steps/s\n", (double) Parameters->BTO4P ); |
||
| 132 | printf( "Button reset timeout - %.0f ms\n", (double) Parameters->BTimeoutR ); |
||
| 133 | printf( "Button reset operation speed - %.2f steps/s\n", (double) Parameters->MinP ); |
||
| 134 | printf( "Backlash operation distance - %d steps\n", (int)Parameters->MaxLoft ); |
||
| 135 | printf( "Revolution distance - %d steps\n", (int)Parameters->RTDelta ); |
||
| 136 | printf( "Minimal revolution distance error - %d steps\n", (int)Parameters->RTMinError ); |
||
| 137 | printf( "Power off temperature - %.2f\xf8\x43\n", (double)Parameters->MaxTemp ); |
||
| 138 | printf( "Duration of the output synchronization pulse - "); |
||
| 139 | if(Parameters->SynOUTP == 0) |
||
| 140 | printf( "minimal\n"); |
||
| 141 | else |
||
| 142 | printf( "%.1f * [Tact Period]\n", Parameters->SynOUTP - 0.5); |
||
| 143 | printf( "Speed of the last phase of the backlash operation - "); |
||
| 144 | if(Parameters->LoftPeriod == 0.0f) |
||
| 145 | printf( "normal\n" ); |
||
| 146 | else |
||
| 147 | printf( "%.2f steps/s\n", (double)Parameters->LoftPeriod ); |
||
| 148 | printf( "<Angular Encoder Step> Equals <Angular Step Motor Step>/<%.2f>\n", Parameters->EncMult); |
||
| 149 | } |
||
| 150 | |||
| 151 | // Function that prints information about device "mode" parameters to console |
||
| 152 | void PrintDMode(USMC_Mode *Mode) { |
||
| 153 | printf( "Mode parameters:\n" ); |
||
| 154 | printf( "Buttons - "); |
||
| 155 | if(Mode->PMode) { |
||
| 156 | printf( "Disabled\n" ); |
||
| 157 | } else { |
||
| 158 | printf( "Enabled\nButton 1 TRUE state - %s\n", Mode->Butt1T?"+3/+5 V":"0 V(GND)" ); |
||
| 159 | printf( "Button 2 TRUE state - %s\n", Mode->Butt2T?"+3/+5 V":"0 V(GND)" ); |
||
| 160 | } |
||
| 161 | printf( "Current reduction regime - %s\n", Mode->PReg?"Used":"Not Used" ); |
||
| 162 | |||
| 163 | if(Mode->ResetD) |
||
| 164 | printf( "Power - %s\n", Mode->EMReset?"Emerjency Off":"Off" ); |
||
| 165 | else |
||
| 166 | printf( "Power - On\n" ); |
||
| 167 | if(Mode->Tr1En || Mode->Tr2En) |
||
| 168 | printf( "Trailers are - %s\n", Mode->TrSwap?"Swapped":"Direct" ); |
||
| 169 | |||
| 170 | printf( "Trailer 1 - "); |
||
| 171 | if(Mode->Tr1En) |
||
| 172 | printf( "Enabled\nTrailer 1 TRUE state - %s\n", Mode->Tr1T?"+3/+5 V":"0 V(GND)" ); |
||
| 173 | else |
||
| 174 | printf( "Disabled\n"); |
||
| 175 | printf( "Trailer 2 - "); |
||
| 176 | if(Mode->Tr2En) |
||
| 177 | printf( "Enabled\nTrailer 2 TRUE state - %s\n", Mode->Tr2T?"+3/+5 V":"0 V(GND)" ); |
||
| 178 | else |
||
| 179 | printf( "Disabled\n"); |
||
| 180 | |||
| 181 | if(Mode->EncoderEn) { |
||
| 182 | printf( "Encoder - Enabled\n"); |
||
| 183 | printf( "Encoder Position Counter is %s\n", Mode->EncoderInv?"Inverted":"Direct"); |
||
| 184 | printf( "Rotary Transducer and Input Syncronisation are\n" |
||
| 185 | " Disabled Because of Encoder\n"); |
||
| 186 | } else { |
||
| 187 | printf( "Encoder - Disabled\n"); |
||
| 188 | printf( "Rotary Transducer - "); |
||
| 189 | if(Mode->RotTeEn) { |
||
| 190 | printf( "Enabled\nRotary Transducer TRUE state - %s\n", Mode->RotTrT?"+3/+5 V":"0 V(GND)" ); |
||
| 191 | printf( "Rotary Transducer Operation - %s\n", Mode->RotTrOp?"Stop on error":"Check and ignore error" ); |
||
| 192 | printf( "Reset Rotary Transducer Check Positions - %s\n", Mode->ResetRT?"Initiated":"No, why?"); |
||
| 193 | } else { |
||
| 194 | printf("Disabled\n"); |
||
| 195 | } |
||
| 196 | printf("Synchronization input mode:\n"); |
||
| 197 | if(Mode->SyncINOp) |
||
| 198 | printf("Step motor will move one time to the destination position\n"); |
||
| 199 | else |
||
| 200 | printf("Step motor will move multiple times by [destination position]\n"); |
||
| 201 | } |
||
| 202 | printf( "Output Syncronization - "); |
||
| 203 | if(Mode->SyncOUTEn) { |
||
| 204 | printf( "Enabled\nReset Output Synchronization Counter - %s\n", Mode->SyncOUTR?"Initiated":"No, why?" ); |
||
| 205 | printf( "Number of steps after which synchronization output sygnal occures - %u\n", Mode->SyncCount ); |
||
| 206 | } else { |
||
| 207 | printf("Disabled\n"); |
||
| 208 | } |
||
| 209 | printf("Synchronization Output is "); |
||
| 210 | if(Mode->SyncInvert) |
||
| 211 | printf("INVERTED\n"); |
||
| 212 | else |
||
| 213 | printf("NORMAL\n"); |
||
| 214 | } |
||
| 215 | |||
| 216 | // Function that prints Encoder state information |
||
| 217 | void PrintEncState(USMC_EncoderState *EnState, USMC_Parameters *up) { |
||
| 218 | /* |
||
| 219 | |Type |Name |Description |
||
| 220 | |--------|-----------|------------------------------------------------------------------------- |
||
| 221 | |Int |EncoderPos |Current position measured by encoder |
||
| 222 | |Int |ECurPos |Current position (in Encoder Steps) - Synchronized with request call |
||
| 223 | |--------|-----------|------------------------------------------------------------------------- |
||
| 224 | */ |
||
| 225 | printf( "The encoder state is:\n" ); |
||
| 226 | printf( "- Current Position in microsteps - %.2f\n", EnState->ECurPos/up->EncMult ); |
||
| 227 | printf( "- Encoder Position in microsteps - %.2f\n\n", EnState->EncoderPos/up->EncMult ); |
||
| 228 | printf( "- Current Position in \"Half of Encoder Step\"s - %d\n", EnState->ECurPos ); |
||
| 229 | printf( "- Encoder Position in \"Half of Encoder Step\"s - %d\n", EnState->EncoderPos ); |
||
| 230 | } |
||
| 231 | |||
| 232 | // Function that prints last error information |
||
| 233 | void PrintError(void) { |
||
| 234 | char er[101]; |
||
| 235 | USMC_GetLastErr(er,100); |
||
| 236 | er[100] = '\0'; |
||
| 237 | printf("\n%s",er); |
||
| 238 | } |
||
| 239 | |||
| 240 | //////////////////////////////////////////////////////////////////////// |
||
| 241 | |||
| 242 | static int ierr; |
||
| 243 | static float Speed=15000.0f; |
||
| 244 | |||
| 245 | #define MAXNODES 4 |
||
| 246 | static USMC_Devices Devices; |
||
| 247 | static USMC_StartParameters DStartPar[MAXNODES]; |
||
| 248 | static USMC_Parameters DPar[MAXNODES]; |
||
| 249 | static USMC_State DState[MAXNODES]; |
||
| 250 | static USMC_EncoderState DEncState[MAXNODES]; |
||
| 251 | static USMC_Mode DMode[MAXNODES]; |
||
| 252 | |||
| 253 | #define ERRLEN 128 |
||
| 254 | static char errstr[ERRLEN]; |
||
| 255 | |||
| 256 | void uSMC_PrintDevices(void) { |
||
| 257 | /* |
||
| 258 | |Type |Name |Description |
||
| 259 | |--------|-----------|------------------------------------------------------------------------- |
||
| 260 | |DWORD |NOD |Number of devices connected to computer |
||
| 261 | |char ** |Serial |Array of pointers to 16–byte ASCII strings of length – NOD |
||
| 262 | |char ** |Version |Array of pointers to 4–byte ASCII strings of length – NOD |
||
| 263 | |--------|-----------|------------------------------------------------------------------------- |
||
| 264 | */ |
||
| 265 | int i; |
||
| 266 | for (i=0; i<Devices.NOD; i++) { |
||
| 267 | printf("Device - %d,\tSerial Number - %.16s,\tVersion - %.4s\n",i+1,Devices.Serial[i],Devices.Version[i]); |
||
| 268 | } |
||
| 269 | } |
||
| 270 | |||
| 271 | int uSMC_SetParameters(int node, int stageType) { |
||
| 272 | /* |
||
| 273 | |Type |Name |Description |
||
| 274 | |--------|-----------|------------------------------------------------------------------------- |
||
| 275 | |float |AccelT |Acceleration time (in ms) |
||
| 276 | |float |DecelT |Deceleration time (in ms) |
||
| 277 | |float |PTimeout |Time (in ms) after which current will be reduced to 60% of normal |
||
| 278 | |float |BTimeout1 |Time (in ms) after which speed of step motor rotation will be equal to the one specified at |
||
| 279 | | | |BTO1P field in this structure |
||
| 280 | |float |BTimeout2 |Time (in ms) after which speed of step motor rotation will be equal to the one specified at |
||
| 281 | | | |BTO2P field in this structure |
||
| 282 | |float |BTimeout3 |Time (in ms) after which speed of step motor rotation will be equal to the one specified at |
||
| 283 | | | |BTO3P field in this structure |
||
| 284 | |float |BTimeout4 |Time (in ms) after which speed of step motor rotation will be equal to the one specified at |
||
| 285 | | | |BTO4P field in this structure |
||
| 286 | |float |BTimeoutR |Time (in ms) after which reset command will be performed (see 5.4.7 at page 53) |
||
| 287 | |float |BTimeoutD |This field is reserved for future use |
||
| 288 | |float |MinP |Speed (steps/sec) while performing reset operation |
||
| 289 | |float |BTO1P |Speed (steps/sec) after BTIMEOUT1 time has passed (see 5.4.8 at page 54) |
||
| 290 | |float |BTO2P |Speed (steps/sec) after BTIMEOUT2 time has passed (see 5.4.8 at page 54) |
||
| 291 | |float |BTO3P |Speed (steps/sec) after BTIMEOUT3 time has passed (see 5.4.8 at page 54) |
||
| 292 | |float |BTO4P |Speed (steps/sec) after BTIMEOUT4 time has passed (see 5.4.8 at page 54) |
||
| 293 | |WORD |MaxLoft |Value in full steps that will be used performing backlash operation |
||
| 294 | |DWORD |StartPos |Current Position saved to FLASH. Refer to test.cpp for implementing this functionality. |
||
| 295 | | | |Should be set to 0 for correct reloading of current position in SMCVieW program |
||
| 296 | |WORD |RTDelta |Revolution distance – number of full steps per one full revolution |
||
| 297 | |WORD |RTMinError |Number of full steps missed to raise the error flag |
||
| 298 | |float |MaxTemp |Maximum allowed temperature (centigrade degrees) |
||
| 299 | |BYTE |SynOUTP |Duration of the output synchronization pulse ( see 5.4.13 at page 58) |
||
| 300 | |float |LoftPeriod |Speed (steps/sec) of the last phase of the backlash operation |
||
| 301 | |float |EncMult |Encoder step multiplier. Should be <Encoder Steps per Revolution> / <SM Steps per Revolution> |
||
| 302 | | | |and should be integer multiplied by 0.25 |
||
| 303 | |--------|-----------|------------------------------------------------------------------------- |
||
| 304 | */ |
||
| 305 | int saveToFlash; |
||
| 306 | int Dev; |
||
| 307 | |||
| 308 | saveToFlash=0; |
||
| 309 | Dev=node-1; |
||
| 310 | if (ierr=USMC_GetParameters(Dev,&DPar[Dev])) return ierr; |
||
| 311 | switch (stageType) { |
||
| 312 | case -1: |
||
| 313 | saveToFlash=1; |
||
| 314 | // DPar[Dev].StartPos = 0; |
||
| 315 | case 1: |
||
| 316 | // Linear stage |
||
| 317 | DPar[Dev].AccelT = 200.0f; |
||
| 318 | DPar[Dev].DecelT = 200.0f; |
||
| 319 | DPar[Dev].PTimeout = 1.0f; |
||
| 320 | DPar[Dev].BTimeout1 = 500.0f; |
||
| 321 | DPar[Dev].BTimeout2 = 500.0f; |
||
| 322 | DPar[Dev].BTimeout3 = 500.0f; |
||
| 323 | DPar[Dev].BTimeout4 = 500.0f; |
||
| 324 | DPar[Dev].BTimeoutR = 500.0f; |
||
| 325 | DPar[Dev].MinP = 60.0f; |
||
| 326 | DPar[Dev].BTO1P = 10.0f; |
||
| 327 | DPar[Dev].BTO2P = 20.0f; |
||
| 328 | DPar[Dev].BTO3P = 30.0f; |
||
| 329 | DPar[Dev].BTO4P = 60.0f; |
||
| 330 | DPar[Dev].MaxLoft = 32; |
||
| 331 | DPar[Dev].RTDelta = 200; |
||
| 332 | DPar[Dev].RTMinError = 15; |
||
| 333 | DPar[Dev].MaxTemp = 70.0f; |
||
| 334 | DPar[Dev].SynOUTP = 1; |
||
| 335 | DPar[Dev].LoftPeriod = 50.0f; |
||
| 336 | DPar[Dev].EncMult = 1.0f; |
||
| 337 | break; |
||
| 338 | case -2: |
||
| 339 | saveToFlash=1; |
||
| 340 | case 2: |
||
| 341 | // Rotation stage |
||
| 342 | DPar[Dev].MaxTemp = 70.0f; |
||
| 343 | DPar[Dev].AccelT = 200.0f; |
||
| 344 | DPar[Dev].DecelT = 200.0f; |
||
| 345 | DPar[Dev].BTimeout1 = 500.0f; |
||
| 346 | DPar[Dev].BTimeout2 = 500.0f; |
||
| 347 | DPar[Dev].BTimeout3 = 500.0f; |
||
| 348 | DPar[Dev].BTimeout4 = 500.0f; |
||
| 349 | DPar[Dev].BTO1P = 100.0f; |
||
| 350 | DPar[Dev].BTO2P = 200.0f; |
||
| 351 | DPar[Dev].BTO3P = 300.0f; |
||
| 352 | DPar[Dev].BTO4P = 600.0f; |
||
| 353 | DPar[Dev].MinP = 500.0f; |
||
| 354 | DPar[Dev].BTimeoutR = 500.0f; |
||
| 355 | DPar[Dev].LoftPeriod = 500.0f; |
||
| 356 | DPar[Dev].RTDelta = 200; |
||
| 357 | DPar[Dev].RTMinError = 15; |
||
| 358 | DPar[Dev].EncMult = 2.5f; |
||
| 359 | DPar[Dev].MaxLoft = 32; |
||
| 360 | DPar[Dev].PTimeout = 100.0f; |
||
| 361 | DPar[Dev].SynOUTP = 1; |
||
| 362 | break; |
||
| 363 | } |
||
| 364 | if (ierr=USMC_SetParameters(Dev,&DPar[Dev])) return ierr; |
||
| 365 | if (saveToFlash) { |
||
| 366 | if(ierr=USMC_SaveParametersToFlash(Dev)) return ierr; |
||
| 367 | PrintDParameters(&DPar[Dev]); |
||
| 368 | printf("\nThese Parameters were Saved to Flash"); |
||
| 369 | } |
||
| 370 | return FALSE; |
||
| 371 | } |
||
| 372 | |||
| 373 | int uSMC_RevertStartPosition(int node) { |
||
| 374 | int Dev; |
||
| 375 | |||
| 376 | Dev=node-1; |
||
| 377 | if (ierr=USMC_GetParameters(Dev,&DPar[Dev])) return ierr; |
||
| 378 | DPar[Dev].StartPos = 0; |
||
| 379 | if (ierr=USMC_SetParameters(Dev,&DPar[Dev])) return ierr; |
||
| 380 | if(ierr=USMC_SaveParametersToFlash(Dev)) return ierr; |
||
| 381 | printf("\nStart Position is Reset to 0\n"); |
||
| 382 | return FALSE; |
||
| 383 | } |
||
| 384 | |||
| 385 | int _VI_FUNC uSMC_PowerOn (int node) { |
||
| 386 | int Dev; |
||
| 387 | |||
| 388 | Dev=node-1; |
||
| 389 | if (ierr=USMC_GetMode(Dev,&DMode[Dev])) return ierr; |
||
| 390 | DMode[Dev].ResetD = FALSE; |
||
| 391 | if (ierr=USMC_SetMode(Dev,&DMode[Dev])) return ierr; |
||
| 392 | return FALSE; |
||
| 393 | } |
||
| 394 | |||
| 395 | int uSMC_PowerOffSave(int node,int saveToFlash) { |
||
| 396 | int Dev=node-1; |
||
| 397 | if (saveToFlash) { |
||
| 398 | if (ierr=USMC_GetParameters(Dev,&DPar[Dev])) return ierr; |
||
| 399 | } |
||
| 400 | if (ierr=USMC_GetMode(Dev,&DMode[Dev])) return ierr; |
||
| 401 | DMode[Dev].ResetD = TRUE; |
||
| 402 | if (ierr=USMC_SetMode(Dev,&DMode[Dev])) return ierr; |
||
| 403 | do { |
||
| 404 | Sleep(50); |
||
| 405 | if (ierr=USMC_GetState(Dev,&DState[Dev])) return ierr; |
||
| 406 | } while(DState[Dev].Power == TRUE); |
||
| 407 | if (saveToFlash) { |
||
| 408 | DPar[Dev].StartPos = DState[Dev].CurPos; |
||
| 409 | if (ierr=USMC_SetParameters(Dev,&DPar[Dev])) return ierr; |
||
| 410 | if (ierr=USMC_SaveParametersToFlash(Dev)) return ierr; |
||
| 411 | } |
||
| 412 | return FALSE; |
||
| 413 | } |
||
| 414 | |||
| 415 | int _VI_FUNC uSMC_PowerOff (int node) { |
||
| 416 | if (ierr=uSMC_PowerOffSave(node,0)) return ierr; |
||
| 417 | return FALSE; |
||
| 418 | } |
||
| 419 | |||
| 420 | int _VI_FUNC uSMC_Open (void) { |
||
| 421 | int i; |
||
| 422 | if (ierr=USMC_Init(&Devices)) return ierr; |
||
| 423 | uSMC_PrintDevices(); |
||
| 424 | return FALSE; |
||
| 425 | } |
||
| 426 | |||
| 427 | int _VI_FUNC uSMC_FindSerial (char serial[]) |
||
| 428 | { |
||
| 429 | int i; |
||
| 430 | for (i=0; i<Devices.NOD; i++) |
||
| 431 | if (!strncmp (Devices.Serial[i], serial, 16)) return i; |
||
| 432 | return -1; |
||
| 433 | } |
||
| 434 | |||
| 435 | int _VI_FUNC uSMC_Reset (int node) { |
||
| 436 | return FALSE; |
||
| 437 | } |
||
| 438 | |||
| 439 | int _VI_FUNC uSMC_Init (int node, int type) { |
||
| 440 | /* |
||
| 441 | |Type |Name |Masked|Saved|Description |
||
| 442 | |--------|-----------|------|-----|------------------------------------------------------------ |
||
| 443 | |BOOL |PMode |– |YES |Turn off buttons (TRUE - buttons disabled) |
||
| 444 | |BOOL |PReg |– |YES |Current reduction regime (TRUE - regime is on) |
||
| 445 | |BOOL |ResetD |– |YES |Turn power off and make a whole step (TRUE - apply) |
||
| 446 | |BOOL |EMReset |– |– |Quick power off (see 5.4.6 at page 53) |
||
| 447 | |BOOL |Tr1T |– |YES |Limit switch 1 TRUE state (TRUE : +3/+5?; FALSE : 0?) |
||
| 448 | |BOOL |Tr2T |– |YES |Limit switch 2 TRUE state (TRUE : +3/+5?; FALSE : 0?) |
||
| 449 | |BOOL |RotTrT |– |YES |Rotary Transducer TRUE state (TRUE : +3/+5?; FALSE : 0?) |
||
| 450 | |BOOL |TrSwap |– |YES |If TRUE, Limit switches are treated to be swapped |
||
| 451 | |BOOL |Tr1En |– |YES |If TRUE Limit switch 1 Operation Enabled |
||
| 452 | |BOOL |Tr2En |– |YES |If TRUE Limit switch 2 Operation Enabled |
||
| 453 | |BOOL |RotTeEn |– |YES |If TRUE Rotary Transducer Operation Enabled |
||
| 454 | |BOOL |RotTrOp |– |YES |Rotary Transducer Operation Select (stop on error if TRUE) |
||
| 455 | |BOOL |Butt1T |– |YES |Button 1 TRUE state (TRUE : +3/+5?; FALSE : 0?) |
||
| 456 | |BOOL |Butt2T |– |YES |Button 2 TRUE state (TRUE : +3/+5?; FALSE : 0?) |
||
| 457 | |BOOL |ResetRT |YES |– |Reset Rotary Transducer Check Positions (need one full revolution |
||
| 458 | | | | | |before it can detect error) |
||
| 459 | |BOOL |SyncOUTEn |– |YES |If TRUE output synchronization enabled |
||
| 460 | |BOOL |SyncOUTR |YES |– |If TRUE output synchronization counter will be reset |
||
| 461 | |BOOL |SyncINOp |– |YES |Synchronization input mode: |
||
| 462 | | | | | |- TRUE - Step motor will move one time to the DestPos |
||
| 463 | | | | | |- FALSE - Step motor will move multiple times by DestPos microsteps as distance |
||
| 464 | |DWORD |SyncCount |– |YES |Number of steps after which synchronization output signal occurs |
||
| 465 | |BOOL |SyncInvert |– |YES |Set this bit to TRUE to invert output synchronization polarity |
||
| 466 | |BOOL |EncoderEn |– |YES |Enable Encoder on pins {SYNCIN,ROTTR} - disables Synchronization input and Rotary Transducer |
||
| 467 | |BOOL |EncoderInv |– |YES |Invert Encoder Counter Direction BOOL ResBEnc YES – Reset <EncoderPos> and <ECurPos> to 0 |
||
| 468 | |BOOL |ResEnc |YES |– |Reset <ECurPos> to <EncoderPos> |
||
| 469 | |--------|-----------|------------------------------------------------------------------------- |
||
| 470 | Masked –> These Boolean values will be automatically cleared by USMC_GetMode (to FALSE) |
||
| 471 | */ |
||
| 472 | int Dev; |
||
| 473 | |||
| 474 | Dev=node-1; |
||
| 475 | // if (ierr=uSMC_PowerOff(node)) return ierr; |
||
| 476 | if (ierr=uSMC_SetParameters(node,type)) return ierr; |
||
| 477 | switch (type) { |
||
| 478 | case -1: |
||
| 479 | case 1: |
||
| 480 | if (ierr=USMC_GetMode(Dev,&DMode[Dev])) return ierr; |
||
| 481 | DMode[Dev].Tr1T = FALSE; |
||
| 482 | DMode[Dev].Tr2T = FALSE; |
||
| 483 | DMode[Dev].Tr1En = TRUE; |
||
| 484 | DMode[Dev].Tr2En = TRUE; |
||
| 485 | DMode[Dev].TrSwap = FALSE; |
||
| 486 | DMode[Dev].EncoderEn = FALSE; |
||
| 487 | if (ierr=USMC_SetMode(Dev,&DMode[Dev])) return ierr; |
||
| 488 | break; |
||
| 489 | } |
||
| 490 | if (ierr=uSMC_PowerOn(node)) return ierr; |
||
| 491 | return FALSE; |
||
| 492 | } |
||
| 493 | |||
| 494 | int _VI_FUNC uSMC_ReferenceMove (int node) { |
||
| 495 | int Dev; |
||
| 496 | float tmpSpeed; |
||
| 497 | |||
| 498 | Dev=node-1; |
||
| 499 | tmpSpeed=Speed; |
||
| 500 | if (ierr=USMC_GetMode(Dev,&DMode[Dev])) return ierr; |
||
| 501 | DMode[Dev].Tr1T = FALSE; |
||
| 502 | DMode[Dev].Tr2T = FALSE; |
||
| 503 | DMode[Dev].Tr1En = TRUE; |
||
| 504 | DMode[Dev].Tr2En = TRUE; |
||
| 505 | DMode[Dev].TrSwap = FALSE; |
||
| 506 | DMode[Dev].EncoderEn = FALSE; |
||
| 507 | if (ierr=USMC_SetMode(Dev,&DMode[Dev])) return ierr; |
||
| 508 | Speed=10000.0f; |
||
| 509 | if (ierr=USMC_SetCurrentPosition(Dev,0)) return ierr; |
||
| 510 | if (ierr=uSMC_MoveTo(node,-500000)) return ierr; |
||
| 511 | if (ierr=uSMC_MoveFor(node,5000)) return ierr; |
||
| 512 | Speed=500.0f; |
||
| 513 | if (ierr=uSMC_MoveTo(node,-500000)) return ierr; |
||
| 514 | Speed=tmpSpeed; |
||
| 515 | if (ierr=uSMC_MoveFor(node,5000)) return ierr; |
||
| 516 | if (ierr=USMC_SetCurrentPosition(Dev,0)) return ierr; |
||
| 517 | return FALSE; |
||
| 518 | } |
||
| 519 | |||
| 520 | int _VI_FUNC uSMC_MoveFor (int node, int dist) { |
||
| 521 | int CurPos; |
||
| 522 | if (ierr=uSMC_GetPosition(node,&CurPos)) return ierr; |
||
| 523 | if (ierr=uSMC_MoveTo(node,CurPos+dist)) return ierr; |
||
| 524 | return FALSE; |
||
| 525 | } |
||
| 526 | |||
| 527 | int _VI_FUNC uSMC_MoveTo (int node, int dest) { |
||
| 528 | /* |
||
| 529 | |Type |Name |Description |
||
| 530 | |--------|-----------|------------------------------------------------------------------------- |
||
| 531 | |BYTE |SDivisor |Step is divided by this factor (1,2,4,8) |
||
| 532 | |BOOL |DefDir |Direction for backlash operation (relative) (see 5.4.15 at page 60) |
||
| 533 | |BOOL |LoftEn |Enable automatic backlash operation (works if slow start/stop mode is off) |
||
| 534 | |BOOL |SlStart |If TRUE slow start/stop mode enabled |
||
| 535 | |BOOL |WSyncIN |If TRUE controller will wait for input synchronization signal to start |
||
| 536 | |BOOL |SyncOUTR |If TRUE output synchronization counter will be reset |
||
| 537 | |BOOL |ForceLoft |If TRUE and destination position is equal to the current position |
||
| 538 | | | |backlash operation will be performed |
||
| 539 | |--------|-----------|------------------------------------------------------------------------- |
||
| 540 | */ |
||
| 541 | int Dev; |
||
| 542 | time_t tcur,tprev; |
||
| 543 | |||
| 544 | Dev=node-1; |
||
| 545 | if (ierr=USMC_GetStartParameters(Dev,&DStartPar[Dev])) return ierr; |
||
| 546 | DStartPar[Dev].SDivisor=8; |
||
| 547 | DStartPar[Dev].SlStart=TRUE; |
||
| 548 | DStartPar[Dev].LoftEn=FALSE; |
||
| 549 | DStartPar[Dev].WSyncIN=FALSE; |
||
| 550 | if (ierr=USMC_Start(Dev,dest,&Speed,&DStartPar[Dev])) return ierr; |
||
| 551 | tprev=time(NULL)+1; |
||
| 552 | do { |
||
| 553 | Delay(0.01); |
||
| 554 | if (ierr=USMC_GetState(Dev,&DState[Dev])) return ierr; |
||
| 555 | tcur=time(NULL); |
||
| 556 | if (tcur > tprev) { |
||
| 557 | printf("%d CurPos %d L%d R%d Rot%d %s %s\n",node, |
||
| 558 | DState[Dev].CurPos,DState[Dev].Trailer1,DState[Dev].Trailer2, DState[Dev].RotTr, |
||
| 559 | DState[Dev].RUN?"Run":"Stopped",DState[Dev].Power?"On":"Off" ); |
||
| 560 | tprev= tcur; |
||
| 561 | } |
||
| 562 | } while (DState[Dev].RUN); |
||
| 563 | return FALSE; |
||
| 564 | } |
||
| 565 | |||
| 566 | int _VI_FUNC uSMC_GetPosition (int node, int *pos) { |
||
| 567 | /* |
||
| 568 | |Type |Name |Description |
||
| 569 | |--------|-----------|------------------------------------------------------------------------- |
||
| 570 | |int |CurPos |
||
| 571 | |float |Temp |Current temperature of the power driver |
||
| 572 | |BYTE |SDivisor |Step is divided by this factor |
||
| 573 | |BOOL |Loft |Indicates backlash status |
||
| 574 | |BOOL |FullPower |Full power if TRUE |
||
| 575 | |BOOL |CW_CCW |Current direction of rotation (relatively to some direction – dependent on |
||
| 576 | | | |step motor circuits connection and on its construction) |
||
| 577 | |BOOL |Power |If TRUE then Step Motor power is ON |
||
| 578 | |BOOL |FullSpeed |If TRUE then full speed. Valid in "Slow Start" mode only |
||
| 579 | |BOOL |AReset |TRUE After Device reset, FALSE after "Set Position" |
||
| 580 | |BOOL |RUN |TRUE if step motor is rotating |
||
| 581 | |BOOL |SyncIN |Logical state directly from input synchronization PIN (pulses treated as positive) |
||
| 582 | |BOOL |SyncOUT |Logical state directly from output synchronization PIN (pulses are positive) |
||
| 583 | |BOOL |RotTr |Indicates current rotary transducer logical press state |
||
| 584 | |BOOL |RotTrErr |Indicates rotary transducer error flag (reset by USMC_SetMode function with ResetRT bit – TRUE) |
||
| 585 | |BOOL |EmReset |Indicates state of emergency disable button (TRUE – Step motor power off) |
||
| 586 | |BOOL |Trailer1 |Indicates Limit switch 1 logical press state |
||
| 587 | |BOOL |Trailer2 |Indicates Limit switch 2 logical press state |
||
| 588 | |float |Voltage |Power supply voltage (Volts) |
||
| 589 | |--------|-----------|------------------------------------------------------------------------- |
||
| 590 | */ |
||
| 591 | int Dev; |
||
| 592 | |||
| 593 | Dev=node-1; |
||
| 594 | if (ierr=USMC_GetState(Dev,&DState[Dev])) return ierr; |
||
| 595 | *pos=DState[Dev].CurPos; |
||
| 596 | return FALSE; |
||
| 597 | } |
||
| 598 | |||
| 599 | void _VI_FUNC uSMC_Close (void) { |
||
| 600 | USMC_Close(); |
||
| 601 | } |
||
| 602 | |||
| 603 | //****************************** |
||
| 604 | |||
| 605 | #ifdef uSMC_MAIN |
||
| 606 | #define uSMC_SERIAL_X "0000000000004925" |
||
| 607 | #define uSMC_SERIAL_Y "0000000000006030" |
||
| 608 | #define uSMC_SERIAL_Z "0000000000002894" |
||
| 609 | # ifdef _WINDOWS |
||
| 610 | int __stdcall WinMain (HINSTANCE hInstance, HINSTANCE hPrevInstance, |
||
| 611 | LPSTR lpszCmdLine, int nCmdShow) { |
||
| 612 | |||
| 613 | const char serials[3][16]={uSMC_SERIAL_X,uSMC_SERIAL_Y,uSMC_SERIAL_Z}; |
||
| 614 | |||
| 615 | int i; |
||
| 616 | int DestPos[3],CurPos[3]; |
||
| 617 | int nodes[3]; |
||
| 618 | // float Speed=2000.0f; |
||
| 619 | |||
| 620 | if (InitCVIRTE (hInstance, 0, 0) == 0) return -1; /* out of memory */ |
||
| 621 | |||
| 622 | uSMC_Open(); |
||
| 623 | for (i=0;i<3;i++) { |
||
| 624 | nodes[i]=uSMC_FindSerial(serials[i])+1; |
||
| 625 | uSMC_Init(nodes[i],1); |
||
| 626 | uSMC_GetPosition(nodes[i],&CurPos[i]); |
||
| 627 | } |
||
| 628 | printf("nodes x,y,z: %2d, %2d, %2d\n",nodes[0],nodes[1],nodes[2]); |
||
| 629 | printf("Current position: %2d, %2d, %2d\n",CurPos[0],CurPos[1],CurPos[2]); |
||
| 630 | // uSMC_ReferenceMove(node); |
||
| 631 | |||
| 632 | getchar(); |
||
| 633 | DestPos[0]=32000; |
||
| 634 | DestPos[1]=125000; |
||
| 635 | DestPos[2]=300000; |
||
| 636 | for (i=0;i<3;i++) { |
||
| 637 | uSMC_MoveTo(nodes[i],DestPos[i]); |
||
| 638 | uSMC_GetPosition(nodes[i],&CurPos[i]); |
||
| 639 | } |
||
| 640 | printf("Current position: %2d, %2d, %2d\n",CurPos[0],CurPos[1],CurPos[2]); |
||
| 641 | |||
| 642 | for (i=0;i<3;i++) uSMC_PowerOff(nodes[i]); |
||
| 643 | getchar(); |
||
| 644 | uSMC_Close(); |
||
| 645 | return 0; |
||
| 646 | } |
||
| 647 | |||
| 648 | # else |
||
| 649 | |||
| 650 | int main (int argc, char **argv) { |
||
| 651 | int i,j,k; |
||
| 652 | int node=0,opt,value=0; |
||
| 653 | int EOldPos=-1; |
||
| 654 | if ( USMC_Init ( &devices ) ) abort(); |
||
| 655 | |||
| 656 | |||
| 657 | while ((opt = getopt(argc, argv, "i:av:f:l:udn:m:s:v:g:h:r:p:")) != -1) { |
||
| 658 | switch (opt) { |
||
| 659 | case 'i': |
||
| 660 | usmc_Init (node, atoi(optarg)); |
||
| 661 | |||
| 662 | break; |
||
| 663 | case 'a': |
||
| 664 | printdevices(devices); |
||
| 665 | for (i=0; i<devices.NOD; i++) { |
||
| 666 | |||
| 667 | USMC_GetState(i,&DState[i]); |
||
| 668 | printf("%d CurPos %d L%d R%d %s %s\t",i, |
||
| 669 | DState[i].CurPos,DState[i].Trailer1,DState[i].Trailer2, |
||
| 670 | DState[i].RUN?"Run":"Stopped",DState[i].Power?"On":"Off" ); |
||
| 671 | USMC_GetParameters(i,&DPar[i]); |
||
| 672 | USMC_GetEncoderState(i,&DEncState[i]); |
||
| 673 | printf("node %d ECurPos 0x%0x %f EncoderPos =0x%04x %f\n",i, |
||
| 674 | DEncState[i].ECurPos, (((DEncState[i].ECurPos)>>5)&0x8FFFFF)/DPar[i].EncMult, |
||
| 675 | DEncState[i].EncoderPos,((DEncState[i].EncoderPos >> 5)&0x8FFFFF)/DPar[i].EncMult); |
||
| 676 | |||
| 677 | } |
||
| 678 | break; |
||
| 679 | |||
| 680 | case 'l': |
||
| 681 | printf("usmc_MoveTo Loop\n"); |
||
| 682 | for (i=0; i<5; i++) { |
||
| 683 | int xpos=i*1000+10000; |
||
| 684 | usmc_MoveTo (1, xpos); |
||
| 685 | for (j=0; j<5; j++) { |
||
| 686 | int ypos=j*1000+10000; |
||
| 687 | usmc_MoveTo (2, ypos); |
||
| 688 | for (k=0; k<50; k++) { |
||
| 689 | int zpos=k*1000+10000; |
||
| 690 | usmc_MoveTo (3, zpos); |
||
| 691 | printf("x=%d y=%d z=%d\n",xpos,ypos,zpos); |
||
| 692 | Delay(atof(optarg)); |
||
| 693 | |||
| 694 | } |
||
| 695 | } |
||
| 696 | } |
||
| 697 | break; |
||
| 698 | case 'n': |
||
| 699 | node = atoi(optarg); |
||
| 700 | break; |
||
| 701 | case 's': |
||
| 702 | usmc_speed = atoi(optarg); |
||
| 703 | break; |
||
| 704 | case 'p': |
||
| 705 | usmc_PowerOnOff(node,atoi(optarg)); |
||
| 706 | break; |
||
| 707 | case 'm': |
||
| 708 | usmc_MoveTo (node, atoi(optarg)); |
||
| 709 | printf("usmc_MoveTo node=%d pos=%d \n",node,atoi(optarg)); |
||
| 710 | |||
| 711 | break; |
||
| 712 | case 'v': |
||
| 713 | value=atoi(optarg); |
||
| 714 | break; |
||
| 715 | /* |
||
| 716 | case 'f': |
||
| 717 | MIKRO_Set (node,optarg,value); |
||
| 718 | printf("MIKRO_Set node %d cmd=%s val=%d\n",node,optarg, value); |
||
| 719 | break; |
||
| 720 | case 'g': |
||
| 721 | MIKRO_Get (node,optarg,&i); |
||
| 722 | printf("MIKRO_Get node %d cmd=%s val=%d\n",node,optarg, i); |
||
| 723 | |||
| 724 | break; |
||
| 725 | |||
| 726 | |||
| 727 | */ |
||
| 728 | |||
| 729 | case 'r': |
||
| 730 | printf("usmc_Reset node=%d mode=%d\n",node, atoi(optarg)); |
||
| 731 | usmc_Reset (node, atoi(optarg)); |
||
| 732 | break; |
||
| 733 | |||
| 734 | case 'h': |
||
| 735 | printf("usmc_ReferenceMove node=%d mode=%d\n",node, atoi(optarg)); |
||
| 736 | usmc_ReferenceMove (node, atoi(optarg)); |
||
| 737 | break; |
||
| 738 | |||
| 739 | case 'u': |
||
| 740 | usmc_RelMove(node, 1000); |
||
| 741 | break; |
||
| 742 | case 'd': |
||
| 743 | usmc_RelMove(node, -1000); |
||
| 744 | break; |
||
| 745 | |||
| 746 | |||
| 747 | default: /* '?' */ |
||
| 748 | help(); |
||
| 749 | |||
| 750 | break; |
||
| 751 | } |
||
| 752 | } |
||
| 753 | if (argc==1) help(); |
||
| 754 | |||
| 755 | |||
| 756 | USMC_Close (); |
||
| 757 | return 0; |
||
| 758 | } |
||
| 759 | |||
| 760 | # endif /* _WINDOWS */ |
||
| 761 | #endif /* _uSMC_MAIN */ |