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
00065
00070 int right( int argc , char * argv[] , void * data)
00071 {
00072 int pwm = atoi(argv[1]);
00073 int current = atoi(argv[2]);
00074 char i = 0;
00075
00076 kmot_SetPoint( motor , kMotRegOpenLoop, pwm);
00077 i = 0;
00078 while(i < 10){
00079 while( kmot_GetMeasure( motor , kMotMesCurrent ) < current)
00080 i++;
00081 }
00082 kmot_SetMode( motor , kMotModeStopMotor );
00083
00084 return 0;
00085 }
00086
00087
00088
00093 int left( int argc , char * argv[] , void * data)
00094 {
00095 int pwm = -atoi(argv[1]);
00096 int current = atoi(argv[2]);
00097 char i = 0;
00098
00099 kmot_SetPoint( motor , kMotRegOpenLoop, pwm);
00100 i = 0;
00101 while(i < 10){
00102 while( kmot_GetMeasure( motor , kMotMesCurrent ) < current)
00103 i++;
00104 }
00105 kmot_SetMode( motor , kMotModeStopMotor );
00106
00107 return 0;
00108 }
00109 int right3( int argc , char * argv[] , void * data)
00110 {
00111
00112 kmot_SetPoint( motor , kMotRegOpenLoop, 2048);
00113 usleep(50000);
00114 kmot_SetMode( motor , kMotModeStopMotor );
00115
00116 return 0;
00117 }
00118
00119
00120
00125 int left3( int argc , char * argv[] , void * data)
00126 {
00127
00128
00129 kmot_SetPoint( motor , kMotRegOpenLoop, -2048);
00130 usleep(50000);
00131 kmot_SetMode( motor , kMotModeStopMotor );
00132
00133 return 0;
00134 }
00135
00136 int right2( int argc , char * argv[] , void * data)
00137 {
00138
00139 kmot_SetPoint( motor , kMotRegOpenLoop, 2048);
00140 usleep(500000);
00141 kmot_SetMode( motor , kMotModeStopMotor );
00142
00143 return 0;
00144 }
00145
00146
00147
00152 int left2( int argc , char * argv[] , void * data)
00153 {
00154
00155
00156 kmot_SetPoint( motor , kMotRegOpenLoop, -2048);
00157 usleep(500000);
00158 kmot_SetMode( motor , kMotModeStopMotor );
00159
00160 return 0;
00161 }
00162
00163
00164
00165
00170 int test( int argc , char * argv[] , void * data)
00171 {
00172 int loop = atoi(argv[1]);
00173
00174 currentCommand = loop;
00175 kmot_SetPoint( motor , kMotRegOpenLoop, loop);
00176 return 0;
00177 }
00178
00179 int help( int argc , char * argv[] , void * data);
00180
00184 static kb_command_t cmds[] = {
00185 { "quit" , 0 , 0 , quit } ,
00186 { "1" , 2 , 2 , right } ,
00187 { "2" , 2 , 2 , left } ,
00188 { "11" , 0 , 0 , right2 } ,
00189 { "12" , 0 , 0 , left2 } ,
00190 { "21" , 0 , 0 , right3 } ,
00191 { "22" , 0 , 0 , left3 } ,
00192 { "3" , 0 , 0 , test } ,
00193 { "help" , 0 , 0 , help } ,
00194 { NULL , 0 , 0 , NULL }
00195 };
00196
00197
00200 int help( int argc , char * argv[] , void * data)
00201 {
00202 kb_command_t * scan = cmds;
00203 while(scan->name != NULL)
00204 {
00205 printf("%s\r\n",scan->name);
00206 scan++;
00207 }
00208 return 0;
00209 }
00210
00211
00215 static char buf[1024];
00216
00217 int main( int argc , char * argv[] )
00218 {
00219 int rc;
00220 char * name;
00221
00222 if ( argc < 2 ) {
00223 kb_msg("usage: kmot_test <motor_name>\n");
00224 kb_msg("motor_name: KoreMotorLE:PriMotor1 (0x1F)\n");
00225 kb_msg(" KoreMotorLE:PriMotor2 (0x20)\n");
00226 kb_msg(" KoreMotorLE:PriMotor3 (0x21)\n");
00227 kb_msg(" KoreMotorLE:PriMotor4 (0x22)\n");
00228 kb_msg(" KoreMotorLE:AltMotor1 (0x23)\n");
00229 kb_msg(" KoreMotorLE:AltMotor2 (0x24)\n");
00230 kb_msg(" KoreMotorLE:AltMotor3 (0x25)\n");
00231 kb_msg(" KoreMotorLE:AltMotor4 (0x26)\n");
00232 return 0;
00233 }
00234
00235 name = argv[1];
00236
00237
00238 kb_set_debug_level(2);
00239
00240 if ((rc = kb_init( argc , argv )) < 0 )
00241 return 1;
00242
00243 signal( SIGINT , ctrlc_handler );
00244
00245 motor = knet_open( name , KNET_BUS_I2C , 0 , NULL );
00246
00247 if ( motor ) {
00248 unsigned int ver , rev;
00249 unsigned char status , error;
00250 int min, max;
00251
00252 kmot_GetFWVersion( motor , &ver );
00253
00254 printf("Motor %s Firmware v%u.%u\n" , name ,
00255 KMOT_VERSION(ver) , KMOT_REVISION(ver) );
00256
00257 kmot_SetMode( motor , kMotModeIdle );
00258 kmot_SetSampleTime( motor , 1550);
00259 kmot_SetMargin( motor , 6 );
00260 kmot_SetOptions( motor , 0x0 , kMotSWOptWindup | kMotSWOptStopMotorBlk );
00261
00262 kmot_ResetError( motor );
00263 kmot_SetBlockedTime( motor , 10 );
00264 kmot_SetLimits( motor , kMotRegCurrent , 0 , 500 );
00265 kmot_SetLimits( motor , kMotRegPos , -10000 , 10000 );
00266
00267
00268 kmot_ConfigurePID( motor , kMotRegSpeed , 768 , 0 , 18 );
00269 kmot_ConfigurePID(motor,kMotRegPos,70,50,10);
00270
00271 kmot_SetSpeedProfile(motor,30,10);
00272
00273
00274 while (!quitReq) {
00275 printf("\n> ");
00276
00277 if ( fgets( buf , sizeof(buf) , stdin ) != NULL ) {
00278 buf[strlen(buf)-1] = '\0';
00279 kb_parse_command( buf , cmds , NULL);
00280 }
00281 }
00282
00283 knet_close( motor );
00284 } else {
00285 printf("Cannot open KoreMotor device\r\n");
00286 }
00287
00288 return 0;
00289 }
00290
00291
00292 extern void kmot_GetSpeedMultiplier( knet_dev_t * dev , unsigned short *mult );
00293
00294 extern void kmot_SetSpeedMultiplier( knet_dev_t * dev , int mode );
00295