00001
00015 #include <korebot/korebot.h>
00016
00017
00018 static int quitReq = 0;
00019
00020
00021
00024 static knet_dev_t * Arm;
00025 static knet_dev_t * Gripper;
00026
00027
00028
00029
00030
00031
00039 int initGripper( void )
00040 {
00041
00042 kgripper_init();
00043
00044 Arm = knet_open( "Kgripper:Arm" , KNET_BUS_I2C , 0 , NULL );
00045 Gripper = knet_open( "Kgripper:Gripper" , KNET_BUS_I2C , 0 , NULL );
00046
00047 if(Arm!=0)
00048 {
00049 if(Gripper!=0)
00050 {
00051 return 0;
00052 }
00053 else
00054 return -1;
00055 }
00056
00057 return -2;
00058
00059 }
00060
00061
00062
00063
00066 int revisionOS( int argc, char * argv[], void * data)
00067 {
00068 unsigned char version, revision ;
00069
00070
00071 version = kgripper_Arm_Get_Version( Arm );
00072 revision = version & 0x0F;
00073 version = ((version & 0xF0) >> 4);
00074 printf("KheperaIII Gripper, ARM Firmware v%X-%2x" , version, revision);
00075 version = kgripper_Gripper_Get_Version( Gripper );
00076 revision = version & 0x0F;
00077 version = ((version & 0xF0) >> 4);
00078 printf(", Gripper Firmware v%X-%2x" , version, revision);
00079
00080
00081 }
00082
00083
00086 int status( int argc, char * argv[], void * data)
00087 {
00088 unsigned char Data8, PosH, PosL;
00089 signed char DataMin8;
00090 unsigned short Data16, Min_Position, Max_Position,i;
00091 float Voltage;
00092
00093 for(i = 0; i< atoi(argv[1]); i++)
00094 {
00095 printf("%c",0x0C);
00096 printf("ARM Status\n");
00097 Data16 = kgripper_Arm_Get_Position( Arm );
00098 printf(" Position %4u\n",Data16);
00099 DataMin8 = kgripper_Arm_Get_Speed( Arm );
00100 printf(" Speed %4d\n",DataMin8);
00101 Data16 = kgripper_Arm_Get_Current(Arm);
00102 printf(" Current %4u\n",Data16);
00103 Data8 = kgripper_Arm_OnTarget(Arm);
00104 printf(" On Target %4u\n",Data8);
00105 Data16 = kgripper_Arm_Get_Voltage(Arm);
00106 Voltage = (float)Data16 / 102.3;
00107 printf(" Voltage %1.2f\n",Voltage);
00108 Data8 = kgripper_Arm_Get_Capacity(Arm);
00109 printf(" Capacity %3u%%\n",Data8);
00110 kgripper_Arm_Get_Limits(Arm, &Min_Position , &Max_Position );
00111 printf(" Limits %4u to %4u\n", Min_Position, Max_Position);
00112 Data16 = kgripper_Arm_Get_Order(Arm);
00113 printf(" Order %4u\n",Data16);
00114 Data8 = kgripper_Arm_Get_Max_Speed(Arm);
00115 printf(" Max Speed %4u\n\n",Data8);
00116
00117 printf("Gripper Status\n");
00118 Data8 = kgripper_Gripper_Get_Position( Gripper );
00119 printf(" Position %4u\n",Data8);
00120 DataMin8 = kgripper_Gripper_Get_Speed( Gripper );
00121 printf(" Speed %4d\n",DataMin8);
00122 Data16 = kgripper_Gripper_Get_Current(Gripper);
00123 printf(" Current %4u\n",Data16);
00124 Data8 = kgripper_Gripper_OnTarget(Gripper);
00125 printf(" On Target %4u\n",Data8);
00126 Data8 = kgripper_Gripper_Get_Limits(Gripper);
00127 printf(" Limits %4u\n", Data8);
00128 Data16 = kgripper_Gripper_Get_Order(Gripper);
00129 printf(" Order %4u\n",Data16);
00130 Data16 = kgripper_Gripper_Get_Torque(Gripper);
00131 printf(" Max Torque %4u\n\n",Data16);
00132 usleep(80000);
00133
00134
00135 }
00136
00137
00138 }
00139
00140
00143 int movearm( int argc, char * argv[], void * data)
00144 {
00145 printf("Move Arm gripper to position %4u\n",atoi(argv[1]));
00146 kgripper_Arm_Set_Order( Arm, atoi(argv[1]));
00147
00148
00149 }
00150
00151
00154 int movegrip( int argc, char * argv[], void * data)
00155 {
00156 printf("Move Finger gripper to position %4u\n",atoi(argv[1]));
00157 kgripper_Gripper_Set_Order( Gripper, atoi(argv[1]));
00158
00159
00160 }
00161
00162
00163
00166 int get( int argc, char * argv[], void * data)
00167 {
00168 unsigned short Min_Position, Max_Position, Data16;
00169
00170 if(atoi(argv[1]) == 0)
00171 {
00172 printf("Release an object\n");
00173 kgripper_Arm_Get_Limits(Arm, &Min_Position , &Max_Position );
00174 kgripper_Arm_Set_Order( Arm, Max_Position-5);
00175 usleep(10000);
00176 while(kgripper_Arm_OnTarget(Arm) == 0);
00177 kgripper_Gripper_Set_Order( Gripper, kgripper_Gripper_Get_Limits(Gripper));
00178 usleep(10000);
00179 while(kgripper_Gripper_OnTarget(Gripper) == 0);
00180 kgripper_Arm_Set_Order( Arm, Min_Position);
00181 usleep(10000);
00182 while(kgripper_Arm_OnTarget(Arm) == 0);
00183 kgripper_Gripper_Set_Order( Gripper, 10);
00184 usleep(10000);
00185 while(kgripper_Gripper_OnTarget(Gripper) == 0);
00186 }
00187 else
00188 {
00189 printf("Get an object\n");
00190 kgripper_Gripper_Set_Order( Gripper, kgripper_Gripper_Get_Limits(Gripper));
00191 usleep(10000);
00192 while(kgripper_Gripper_OnTarget(Gripper) == 0);
00193 kgripper_Arm_Get_Limits(Arm, &Min_Position , &Max_Position );
00194 kgripper_Arm_Set_Order( Arm, Max_Position);
00195 usleep(10000);
00196 while(kgripper_Arm_OnTarget(Arm) == 0);
00197 while(kgripper_Gripper_Object_Detected(Gripper) == 0);
00198 kgripper_Gripper_Set_Torque( Gripper, 250);
00199 kgripper_Gripper_Set_Order( Gripper, 0);
00200 usleep(100000);
00201 while(kgripper_Gripper_Get_Current( Gripper) < 250);
00202 kgripper_Arm_Set_Order( Arm, Min_Position);
00203 usleep(10000);
00204 while(kgripper_Arm_OnTarget(Arm) == 0);
00205 Data16 = kgripper_Gripper_Get_Resistivity( Gripper);
00206 printf(" Object resistivity %u\n", Data16);
00207
00208 }
00209
00210
00211 }
00212
00215 int grip( int argc, char * argv[], void * data)
00216 {
00217 unsigned short Data16, loop, n, Grip_Lim;
00218
00219 n = atoi(argv[1]);
00220 if(n == 0)
00221 {
00222 loop = n+1;
00223 printf("Start demo mode. Type CTRL+C to stop\n", n);
00224 }
00225 else
00226 {
00227 loop = n;
00228 printf("Start %5u demo move\n", n);
00229 }
00230 Grip_Lim = kgripper_Gripper_Get_Limits(Gripper);
00231 do
00232 {
00233 kgripper_Gripper_Set_Order( Gripper, 0);
00234 usleep(100000);
00235 while(kgripper_Gripper_Get_Current( Gripper) < kgripper_Gripper_Get_Torque( Gripper ));
00236 kgripper_Gripper_Set_Order( Gripper,Grip_Lim - 20 );
00237 usleep(10000);
00238 while(kgripper_Gripper_OnTarget(Gripper) == 0);
00239
00240 if(n != 0)
00241 loop--;
00242
00243 }while(loop > 0);
00244
00245
00246 }
00247
00250 int demo( int argc, char * argv[], void * data)
00251 {
00252 unsigned short Min_Position, Max_Position, Data16, loop, n, Grip_Lim;
00253
00254 n = atoi(argv[1]);
00255 if(n == 0)
00256 {
00257 loop = n+1;
00258 printf("Start demo mode. Type CTRL+C to stop\n", n);
00259 }
00260 else
00261 {
00262 loop = n;
00263 printf("Start %5u demo move\n", n);
00264 }
00265 kgripper_Gripper_Set_Torque( Gripper, 300);
00266 kgripper_Arm_Get_Limits(Arm, &Min_Position , &Max_Position );
00267 Grip_Lim = kgripper_Gripper_Get_Limits(Gripper);
00268 do
00269 {
00270
00271 kgripper_Arm_Set_Order( Arm, Max_Position);
00272 kgripper_Gripper_Set_Order( Gripper,Grip_Lim - 20 );
00273 usleep(10000);
00274 while(kgripper_Gripper_OnTarget(Gripper) == 0);
00275 kgripper_Gripper_Set_Order( Gripper, 0);
00276 usleep(100000);
00277 while(kgripper_Gripper_Get_Current( Gripper) < 300);
00278 while(kgripper_Arm_OnTarget(Arm) == 0);
00279 kgripper_Arm_Set_Order( Arm, 600);
00280 kgripper_Gripper_Set_Order( Gripper,Grip_Lim - 20 );
00281 usleep(10000);
00282 while(kgripper_Gripper_OnTarget(Gripper) == 0);
00283 while(kgripper_Arm_OnTarget(Arm) == 0);
00284 kgripper_Arm_Set_Order( Arm, Min_Position);
00285 usleep(10000);
00286 while(kgripper_Arm_OnTarget(Arm) == 0);
00287 kgripper_Gripper_Set_Order( Gripper, 10);
00288 usleep(10000);
00289 while(kgripper_Gripper_OnTarget(Gripper) == 0);
00290
00291 if(n != 0)
00292 loop--;
00293
00294 }while(loop > 0);
00295
00296
00297 }
00298
00299
00302 int sensor( int argc, char * argv[], void * data)
00303 {
00304 unsigned char Data8;
00305 unsigned short Data16, Dist_IR_Left, Dist_IR_Right,i;
00306
00307
00308 for(i = 0; i< atoi(argv[1]); i++)
00309 {
00310 printf("%c",0x0C);
00311 printf("Finger Sensors value\n");
00312 kgripper_Gripper_Get_Distance_Sensors( Gripper , &Dist_IR_Left , &Dist_IR_Right );
00313 printf(" Distance Left = %4u Right = %4u\n",Dist_IR_Left , Dist_IR_Right);
00314 kgripper_Gripper_Get_Ambiant_IR_Light( Gripper , &Dist_IR_Left , &Dist_IR_Right );
00315 printf(" Ambiant light Left = %4u Right = %4u\n",Dist_IR_Left , Dist_IR_Right);
00316 Data8 = kgripper_Gripper_Object_Detected(Gripper);
00317 printf(" Object detection %u\n", Data8);
00318 Data16 = kgripper_Gripper_Get_Resistivity( Gripper);
00319 printf(" Object resistivity %u\n", Data16);
00320 usleep(80000);
00321
00322
00323 }
00324
00325
00326 }
00327
00328
00329
00332 int setTorque( int argc, char * argv[], void * data)
00333 {
00334 printf("Maximal Torque set at %4u\n",atoi(argv[1]));
00335 kgripper_Gripper_Set_Torque( Gripper, atoi(argv[1]));
00336
00337 }
00338
00339
00342 int setMaxSpeed( int argc, char * argv[], void * data)
00343 {
00344 printf("Maximal speed set at %2u\n",atoi(argv[1]));
00345 kgripper_Arm_Set_Max_Speed( Arm, atoi(argv[1]));
00346
00347 }
00348
00349
00352 int searchLimit( int argc, char * argv[], void * data)
00353 {
00354 unsigned short Min_Position, Max_Position;
00355 unsigned char Data8;
00356 printf("Search the maximal mechanical limits\n\r");
00357 kgripper_Arm_Set_Search_Limit( Arm, 1);
00358 kgripper_GripperSet_Search_Limit( Gripper, 1);
00359 while(kgripper_Arm_Get_Search_Limit(Arm) || kgripper_Gripper_Get_Search_Limit(Gripper))
00360 {
00361 sleep(1);
00362 }
00363 kgripper_Arm_Get_Limits(Arm, &Min_Position , &Max_Position );
00364 Data8 = kgripper_Gripper_Get_Limits(Gripper);
00365 printf("New mechanical Limits: Arm = %4u to %4u\n", Min_Position, Max_Position);
00366 printf(" Gripper = %4u\n",Data8);
00367
00368 }
00369
00370
00373 int quit( int argc , char * argv[] , void * data)
00374 {
00375 quitReq = 1;
00376 }
00377
00378
00379 int help( int argc , char * argv[] , void * data);
00380
00384 static kb_command_t cmds[] = {
00385 { "quit" , 0 , 0 , quit } ,
00386 { "exit" , 0 , 0 , quit } ,
00387 { "status" , 1 , 1 , status },
00388 { "getrev" , 0 , 0 , revisionOS },
00389 { "movearm" , 1 , 1 , movearm } ,
00390 { "movegrip" , 1 , 1 , movegrip } ,
00391 { "sensor" , 1 , 1 , sensor } ,
00392 { "maxspeed" , 1 , 1 , setMaxSpeed } ,
00393 { "get" , 1 , 1 , get } ,
00394 { "demo" , 1 , 1 , demo } ,
00395 { "grip" , 1 , 1 , grip } ,
00396 { "torque" , 1 , 1 , setTorque } ,
00397 { "search" , 0 , 0 , searchLimit },
00398 { "help" , 0 , 0 , help } ,
00399 { NULL , 0 , 0 , NULL }
00400 };
00401
00402
00405 int help( int argc , char * argv[] , void * data)
00406 {
00407 kb_command_t * scan = cmds;
00408 while(scan->name != NULL)
00409 {
00410 printf("%s\r\n",scan->name);
00411 scan++;
00412 }
00413 return 0;
00414 }
00415
00416
00417
00420 int main( int arc, char *argv[])
00421 {
00422 char i;
00423
00424 char buf[64];
00425
00426 printf("Khepera3 Gripper test program (C) K-Team S.A\r\n");
00427
00428 if(!initGripper())
00429 {
00430 printf("Init oke...\r\n");
00431
00432 while (!quitReq)
00433 {
00434
00435
00436 printf("\n> ");
00437
00438 if ( fgets( buf , sizeof(buf) , stdin ) != NULL )
00439 {
00440 buf[strlen(buf)-1] = '\0';
00441 kb_parse_command( buf , cmds , NULL);
00442 }
00443 }
00444
00445 printf("Exiting...\r\n");
00446 }
00447 else
00448 printf("Fatal error, unable to initialize\r\n");
00449
00450 }
00451
00452