00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011 #include <signal.h>
00012 #include <korebot/korebot.h>
00013
00032 static int quitReq = 0;
00033 static int stopReq = 0;
00034 static int currentCommand = 0;
00035
00036 static FILE * logfile;
00037 static knet_dev_t * motor;
00038 static pthread_t log_task;
00039
00043 static void ctrlc_handler( int sig )
00044 {
00045 stopReq = 1;
00046 }
00047
00048
00051 int quit( int argc , char * argv[] , void * data)
00052 {
00053 quitReq = 1;
00054 }
00055
00056
00059 int stop( int argc , char * argv[] , void * data)
00060 {
00061 kmot_SetMode( motor , kMotModeStopMotor );
00062 }
00063
00064
00068 int setsampletime( int argc , char * argv[] , void * data)
00069 {
00070 int sampletime = atoi( argv[1] );
00071
00072 printf("Set sample time to %d\n" , sampletime );
00073
00074 kmot_SetSampleTime( motor , sampletime );
00075 }
00076
00077
00082 int configspeedprofile( int argc , char * argv[] , void * data)
00083 {
00084 int vmax , accel;
00085
00086 vmax = atoi(argv[1]);
00087 accel = atoi(argv[2]);
00088
00089 printf("Set speed profile (Vmax,accel) = (%d,%d)\n" , vmax , accel );
00090 kmot_SetSpeedProfile( motor , vmax , accel );
00091 }
00092
00093
00099 int setpid( int argc , char * argv[] , void * data)
00100 {
00101 int regType[] = {
00102 kMotRegPos ,
00103 kMotRegPosProfile ,
00104 kMotRegSpeed ,
00105 kMotRegSpeedProfile ,
00106 kMotRegTorque
00107 };
00108
00109 char * regTypeStr[] = {
00110 "pos" ,
00111 "posprofile" ,
00112 "speed" ,
00113 "speedprofile" ,
00114 "torque" ,
00115 NULL
00116 };
00117
00118 int reg, kp, ki, kd;
00119
00120 for (reg=0; regTypeStr[reg] != NULL; reg++) {
00121 if (!strcasecmp( argv[1] , regTypeStr[reg] )) {
00122
00123 kp = atoi(argv[2]);
00124 ki = atoi(argv[3]);
00125 kd = atoi(argv[4]);
00126
00127 printf("Set (P,I,D) to (%d,%d,%d)\n" , kp , ki , kd );
00128 kmot_ConfigurePID( motor , regType[reg] , kp , kd , ki );
00129 }
00130 }
00131 }
00132
00133
00134
00135 #define timercpy(a,b) \
00136 (a)->tv_sec = (b)->tv_sec; \
00137 (a)->tv_usec = (b)->tv_usec;
00138
00139
00142 void * kmot_log_task(void * arg)
00143 {
00144 int32_t position,speed,current;
00145
00146 pthread_setcancelstate(PTHREAD_CANCEL_ENABLE,NULL);
00147
00148 logfile = fopen(arg,"w+");
00149 if(!logfile)
00150 {
00151 fprintf(stderr,"Error: Unable to open %s, no data logging.\r\n",arg);
00152 pthread_exit(NULL);
00153 }
00154 printf("Opened logfile: %s\r\n",arg);
00155
00156
00157
00158 while(1)
00159 {
00160 #if 0
00161 do
00162 {
00163 gettimeofday(&clock,&timez);
00164 timersub(&clock,&clocksav,&clockdif);
00165 }
00166 while(clockdif.tv_usec < 500);
00167
00168 timercpy(&clocksav,&clock);
00169 #endif
00170 fprintf(logfile,"%d\t",currentCommand);
00171
00172
00173 speed = kmot_GetMeasure(motor, kMotMesSpeed);
00174 fprintf(logfile,"%ld\t",speed);
00175 position = kmot_GetMeasure(motor, kMotMesPos);
00176 fprintf(logfile,"%ld\t",position);
00177 current = kmot_GetMeasure(motor, kMotMesCurrent);
00178 fprintf(logfile,"%ld\r\n",current);
00179
00180
00181
00182
00183
00184
00185 fflush(logfile);
00186 }
00187
00188 }
00189
00190
00193 int stoplog(int argc, char * argv[], void * data)
00194 {
00195 if(pthread_cancel(log_task))
00196 {
00197 KB_FATAL("stoplog", KB_ERROR_PTHREAD, "kmot_log_task");
00198 }
00199 }
00200
00201
00204 int startlog(int argc, char * argv[], void * data)
00205 {
00206 const char * defaultfile = "motorlog.dat";
00207
00208 if(argc == 2)
00209 {
00210 if(pthread_create(&log_task, NULL, kmot_log_task, (void*)argv[1]))
00211 {
00212 KB_FATAL("startlog", KB_ERROR_PTHREAD, "kmot_log_task");
00213 }
00214 }
00215 else
00216 if(pthread_create(&log_task, NULL, kmot_log_task, (void*)defaultfile))
00217 {
00218 KB_FATAL("startlog", KB_ERROR_PTHREAD, "kmot_log_task");
00219 }
00220 }
00221
00222
00227 int init( int argc , char * argv[] , void * data)
00228 {
00229 kmot_SetMode( motor , kMotModeIdle );
00230 kmot_SetSampleTime( motor , 1550);
00231 kmot_SetMargin( motor , 6 );
00232 kmot_SetOptions( motor , 0x0 , kMotSWOptWindup | kMotSWOptStopMotorBlk );
00233
00234 kmot_ResetError( motor );
00235 kmot_SetBlockedTime( motor , 10 );
00236 kmot_SetLimits( motor , kMotRegCurrent , 0 , 500 );
00237 kmot_SetLimits( motor , kMotRegPos , -10000 , 10000 );
00238
00239
00240 kmot_ConfigurePID( motor , kMotRegSpeed , 768 , 0 , 18 );
00241 kmot_ConfigurePID(motor,kMotRegPos,70,50,10);
00242
00243 kmot_SetSpeedProfile(motor,30,10);
00244 return 0;
00245 }
00246
00247
00252 int setspeed( int argc , char * argv[] , void * data)
00253 {
00254 int speed = atoi(argv[1]);
00255
00256 currentCommand = speed;
00257 kmot_SetPoint( motor , kMotRegSpeed , speed);
00258 return 0;
00259 }
00260
00261
00268 int setspeedprofile( int argc , char * argv[] , void * data)
00269 {
00270 int speed = atoi(argv[1]);
00271
00272 currentCommand = speed;
00273 kmot_SetPoint( motor , kMotRegSpeedProfile , speed);
00274
00275 return 0;
00276 }
00277
00278
00279
00280
00281
00282
00291 int velprescaler( int argc , char * argv[] , void * data)
00292 {
00293 int prescale = atoi(argv[1]);
00294
00295 currentCommand = prescale;
00296 kmot_SetVelocityPrescale( motor , prescale);
00297 return 0;
00298 }
00299
00300
00305 int prescaler( int argc , char * argv[] , void * data)
00306 {
00307 int prescale = atoi(argv[1]);
00308
00309 currentCommand = prescale;
00310 kmot_SetPrescale( motor , prescale);
00311 return 0;
00312 }
00313
00314
00319 int setmultiplier( int argc , char * argv[] , void * data)
00320 {
00321 int mult = atoi(argv[1]);
00322
00323 currentCommand = mult;
00324
00325 kmot_SetSpeedMultiplier( motor, mult );
00326 return 0;
00327 }
00328
00329
00334 int multiplier( int argc, char * argv[], void * data )
00335 {
00336 short mult;
00337
00338 kmot_GetSpeedMultiplier( motor , &mult );
00339
00340 printf("speed multiplier = %d\r\n", mult);
00341 }
00342
00343
00344
00349 int settorque( int argc , char * argv[] , void * data)
00350 {
00351 int torque = atoi(argv[1]);
00352
00353 currentCommand = torque;
00354 kmot_SetPoint( motor , kMotRegTorque , torque);
00355 return 0;
00356 }
00357
00358
00363 int openloop( int argc , char * argv[] , void * data)
00364 {
00365 int loop = atoi(argv[1]);
00366
00367 currentCommand = loop;
00368 kmot_SetPoint( motor , kMotRegOpenLoop, loop);
00369 return 0;
00370 }
00371
00372
00373
00378 int setpos( int argc , char * argv[] , void * data)
00379 {
00380 kmot_SetPosition( motor , atoi(argv[1]) );
00381
00382 return 0;
00383 }
00384
00385
00392 int moveat( int argc , char * argv[] , void * data)
00393 {
00394 int pos = atoi(argv[1]);
00395
00396 currentCommand = pos;
00397
00398 kmot_SetPoint( motor , kMotRegPos, pos);
00399
00400 return 0;
00401 }
00402
00403
00410 int moveatprofile( int argc , char * argv[] , void * data)
00411 {
00412 int pos = atoi(argv[1]);
00413
00414 currentCommand = pos;
00415 kmot_SetPoint( motor , kMotRegPosProfile , pos);
00416
00417 return 0;
00418 }
00419
00420
00421
00425 int measure( int argc , char * argv[] , void * data)
00426 {
00427 int v[3];
00428
00429 v[0] = kmot_GetMeasure( motor , kMotMesPos );
00430 v[1] = kmot_GetMeasure( motor , kMotMesSpeed );
00431 v[2] = kmot_GetMeasure( motor , kMotMesCurrent );
00432
00433 printf( "pos: %d, speed: %d, current: %d\n" ,
00434 v[0] , v[1] , v[2] );
00435 return 0;
00436 }
00437
00438
00444 int test( int argc , char * argv[] , void * data)
00445 {
00446 int curPos;
00447 int deltaPos = atoi(argv[1]);
00448
00449 stopReq = 0;
00450
00451 curPos = kmot_GetMeasure( motor , kMotMesPos );
00452
00453
00454 while ( !stopReq ) {
00455 kmot_SetPoint( motor , kMotRegPos , curPos + deltaPos );
00456 usleep( 200000 );
00457
00458 status ( 0 , NULL );
00459
00460 kmot_SetPoint( motor , kMotRegPos , curPos );
00461 usleep( 200000 );
00462
00463 status ( 0 , NULL );
00464
00465 kmot_SetPoint( motor , kMotRegPos , curPos - deltaPos );
00466 usleep ( 200000 );
00467
00468 status( 0 , NULL );
00469
00470 }
00471
00472 kmot_SetPoint( motor , kMotRegPos , curPos );
00473
00474 stopReq = 0;
00475 return 0;
00476 }
00477
00478
00479
00482 int statusclear( int argc , char * argv[] , void * data)
00483 {
00484 kmot_ResetError(motor);
00485 }
00486
00487
00492 int setoption( int argc , char * argv[] , void * data)
00493 {
00494 kmot_SetOptions( motor , atoi(argv[1]), atoi(argv[2]));
00495
00496 return 0;
00497 }
00498
00499
00500
00501
00502
00507 int option( int argc , char * argv[] , void * data)
00508 {
00509 unsigned char software , hardware;
00510
00511 kmot_GetOptions( motor , &software, &hardware);
00512
00513 printf( "hardware=%02X software=%02X\n" ,
00514 hardware , software );
00515
00516 if ( hardware & kMotHWOptNormal)
00517 printf( "Normal Mode\n" );
00518 else
00519 printf( "Idle Mode\n" );
00520
00521 if ( hardware & kMotHWOptAnSetPtInEn)
00522 printf("Analog Setpoint option active\r\n");
00523
00524 if ( hardware & kMotHWOptEncRes1x)
00525 printf( "Encoder resolution set to 100%\n" );
00526 else
00527 printf( "Encoder resolution set to 25%\n" );
00528
00529 if ( hardware & kMotHWOptTorqueInv)
00530 printf( "Torque Inversion option active\n" );
00531
00532 if ( software & kMotSWOptSepD)
00533 printf("Separated D option active\r\n");
00534
00535 if ( software & kMotSWOptWindup)
00536 printf("AntiWindup option active\r\n");
00537
00538 if ( software & kMotSWOptSoftStopMin)
00539 printf("Softstop min option active\r\n");
00540
00541 if ( software & kMotSWOptSoftStopMax)
00542 printf("Softstop max option active\r\n");
00543
00544 if ( software & kMotSWOptSoftStopErr)
00545 printf("Softstop error option active\r\n");
00546
00547 if ( software & kMotSWOptStopMotorBlk)
00548 printf("Block Motor option active\r\n");
00549
00550 return 0;
00551 }
00552
00556 int status( int argc , char * argv[] , void * data)
00557 {
00558 unsigned char error , status;
00559
00560 kmot_GetStatus( motor , &status , &error );
00561
00562 printf( "status=%02X error=%02X\n" ,
00563 status , error );
00564
00565 if ( status & kMotStatusMoveDet )
00566 printf( "Movement detect!\n" );
00567
00568 printf( "Direction %s !\n" ,
00569 (status&kMotStatusDir) ? "Negative" : "Positive" );
00570
00571 if ( status & kMotStatusOnSetPt )
00572 printf( "On Set Point!\n" );
00573
00574 if ( status & kMotStatusNearSetPt )
00575 printf( "Near Set Point !\n" );
00576
00577 if ( status & kMotStatusCmdSat )
00578 printf( "Command saturated !\n" );
00579
00580 if ( status & kMotStatusWindup )
00581 printf( "Antireset Wind up active !\n" );
00582
00583 if ( status & kMotStatusSoftCurCtrl )
00584 printf( "Software Current Control Active !\n" );
00585
00586 if ( status & kMotStatusSoftStop )
00587 printf( "Software Stop Active !\n" );
00588
00589 if ( error & kMotErrorSampleTimeTooSmall )
00590 printf( "Sample Time Too Small !\n" );
00591
00592 if ( error & kMotErrorWDTOverflow )
00593 printf( "WatchDot Timer Overflow !\n" );
00594
00595 if ( error & kMotErrorBrownOut )
00596 printf( "Brown-out !\n" );
00597
00598 if ( error & kMotErrorSoftStopMotor )
00599 printf( "Software Stopped Motor !\n" );
00600
00601 if ( error & kMotErrorMotorBlocked )
00602 printf( "Motor Blocked !\n" );
00603
00604 if ( error & kMotErrorPosOutOfRange )
00605 printf( "Position Out of Range !\n" );
00606
00607 if ( error & kMotErrorSpeedOutOfRange )
00608 printf( "Speed Out of Range !\n" );
00609
00610 if ( error & kMotErrorTorqueOutOfRange )
00611 printf( "Torque Out of Range !\n" );
00612
00613 return 0;
00614 }
00615
00616
00617 int help( int argc , char * argv[] , void * data);
00618
00622 static kb_command_t cmds[] = {
00623 { "quit" , 0 , 0 , quit } ,
00624 { "exit" , 0 , 0 , quit } ,
00625 { "bye" , 0 , 0 , quit } ,
00626 { "stop" , 0 , 0 , stop } ,
00627 { "init" , 0 , 0 , init } ,
00628 { "stoplog" , 0 , 0 , stoplog} ,
00629 { "startlog" , 0 , 1 , startlog} ,
00630 { "setspeed" , 1 , 1 , setspeed } ,
00631 { "setspeedprofile" , 1 , 1 , setspeedprofile },
00632 { "settorque" , 1 , 1 , settorque } ,
00633 { "prescaler" , 1 , 1 , prescaler } ,
00634 { "velprescaler" , 1 , 1 , velprescaler } ,
00635 { "openloop" , 1 , 1 , openloop} ,
00636 { "setsampletime" , 1 , 1 , setsampletime } ,
00637 { "configspeedprofile" , 2 , 2 , configspeedprofile } ,
00638 { "setpid" , 4 , 4 , setpid } ,
00639 { "setpos" , 1 , 1 , setpos } ,
00640 { "moveat" , 1 , 1 , moveat } ,
00641 { "moveatprofile" , 1 , 1 , moveatprofile } ,
00642 { "measure" , 0 , 0 , measure } ,
00643 { "status" , 0 , 0 , status } ,
00644 { "option" , 0 , 0 , option } ,
00645 { "setoption" , 2 , 2 , setoption } ,
00646 { "multiplier" , 0 , 0 , multiplier } ,
00647 { "setmultiplier" , 1 , 1 , setmultiplier } ,
00648 { "statusclear" , 0 , 0 , statusclear } ,
00649 { "test" , 1 , 1 , test } ,
00650 { "help" , 0 , 0 , help } ,
00651 { NULL , 0 , 0 , NULL }
00652 };
00653
00654
00657 int help( int argc , char * argv[] , void * data)
00658 {
00659 kb_command_t * scan = cmds;
00660 while(scan->name != NULL)
00661 {
00662 printf("%s\r\n",scan->name);
00663 scan++;
00664 }
00665 return 0;
00666 }
00667
00668
00672 static char buf[1024];
00673
00674 int main( int argc , char * argv[] )
00675 {
00676 int rc;
00677 char * name;
00678
00679 if ( argc < 2 ) {
00680 kb_msg("usage: kmot_test <motor_name>\n");
00681 kb_msg("motor_name: KoreMotorLE:PriMotor1 (0x1F)\n");
00682 kb_msg(" KoreMotorLE:PriMotor2 (0x20)\n");
00683 kb_msg(" KoreMotorLE:PriMotor3 (0x21)\n");
00684 kb_msg(" KoreMotorLE:PriMotor4 (0x22)\n");
00685 kb_msg(" KoreMotorLE:AltMotor1 (0x23)\n");
00686 kb_msg(" KoreMotorLE:AltMotor2 (0x24)\n");
00687 kb_msg(" KoreMotorLE:AltMotor3 (0x25)\n");
00688 kb_msg(" KoreMotorLE:AltMotor4 (0x26)\n");
00689 return 0;
00690 }
00691
00692 name = argv[1];
00693
00694
00695 kb_set_debug_level(2);
00696
00697 if ((rc = kb_init( argc , argv )) < 0 )
00698 return 1;
00699
00700 signal( SIGINT , ctrlc_handler );
00701
00702 motor = knet_open( name , KNET_BUS_I2C , 0 , NULL );
00703
00704 if ( motor ) {
00705 unsigned int ver , rev;
00706 unsigned char status , error;
00707 int min, max;
00708
00709 kmot_GetFWVersion( motor , &ver );
00710
00711 printf("Motor %s Firmware v%u.%u\n" , name ,
00712 KMOT_VERSION(ver) , KMOT_REVISION(ver) );
00713
00714 while (!quitReq) {
00715 printf("\n> ");
00716
00717 if ( fgets( buf , sizeof(buf) , stdin ) != NULL ) {
00718 buf[strlen(buf)-1] = '\0';
00719 kb_parse_command( buf , cmds , NULL);
00720 }
00721 }
00722
00723 knet_close( motor );
00724 } else {
00725 printf("Cannot open KoreMotor device\r\n");
00726 }
00727
00728 return 0;
00729 }
00730
00731
00732 extern void kmot_GetSpeedMultiplier( knet_dev_t * dev , unsigned short *mult );
00733
00734 extern void kmot_SetSpeedMultiplier( knet_dev_t * dev , int mode );
00735