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