00001
00015 #include "korebot.h"
00016
00017
00028 int kgripper_init( void )
00029 {
00030 int rc;
00031
00032
00033
00034 if((rc = kb_init( 0 , NULL )) < 0 )
00035 {
00036
00037 KB_ERROR("kb_kh3_init",KB_ERROR_KH3KBINIT);
00038 return KH3_ERROR_KBINIT;
00039 }
00040 }
00041
00042
00043
00044
00053 unsigned char kgripper_Arm_Get_Version( knet_dev_t * dev )
00054 {
00055 unsigned char OSVersion;
00056 knet_read8( dev , ARM_VERSION , &OSVersion );
00057
00058 return OSVersion;
00059
00060 }
00061
00062
00073 unsigned short kgripper_Arm_Get_Position( knet_dev_t * dev )
00074 {
00075 unsigned short Position;
00076 knet_read16( dev , ARM_POSITION , &Position );
00077
00078 return Position;
00079
00080 }
00081
00082
00091 unsigned char kgripper_Arm_Get_Speed( knet_dev_t * dev )
00092 {
00093 unsigned char Speed;
00094 knet_read8( dev , ARM_SPEED , &Speed );
00095
00096 return Speed;
00097
00098 }
00099
00100
00108 unsigned short kgripper_Arm_Get_Current( knet_dev_t * dev )
00109 {
00110 unsigned short Current;
00111 knet_read16( dev , ARM_CURRENT , &Current );
00112
00113 return Current;
00114
00115 }
00116
00117
00126 unsigned char kgripper_Arm_OnTarget( knet_dev_t * dev )
00127 {
00128 unsigned char OnTarget;
00129 knet_read8( dev , ARM_ON_TARGET , &OnTarget );
00130
00131 return OnTarget;
00132
00133 }
00134
00135
00143 unsigned short kgripper_Arm_Get_Voltage( knet_dev_t * dev )
00144 {
00145 unsigned short Voltage;
00146 knet_read16( dev , ARM_VOLTAGE , &Voltage );
00147
00148 return Voltage;
00149
00150 }
00151
00152
00161 unsigned char kgripper_Arm_Get_Capacity( knet_dev_t * dev )
00162 {
00163 unsigned char Capacity;
00164 knet_read8( dev , ARM_CAPACITY , &Capacity );
00165
00166 return Capacity;
00167
00168 }
00169
00170
00180 void kgripper_Arm_Get_Limits( knet_dev_t * dev ,
00181 unsigned short * Min_Position ,
00182 unsigned short * Max_Position )
00183 {
00184
00185 if (Min_Position != NULL )
00186 knet_read16( dev , ARM_MIN_POSITION , Min_Position );
00187
00188 if ( Max_Position != NULL )
00189 knet_read16( dev , ARM_MAX_POSITION , Max_Position );
00190 }
00191
00192
00200 unsigned short kgripper_Arm_Get_Order( knet_dev_t * dev )
00201 {
00202 unsigned short Order;
00203 knet_read16( dev , ARM_ORDER , &Order );
00204
00205 return Order;
00206
00207 }
00208
00209
00218 unsigned char kgripper_Arm_Get_Max_Speed( knet_dev_t * dev )
00219 {
00220 unsigned char Max_Speed;
00221 knet_read8( dev , ARM_MAX_SPEED , &Max_Speed );
00222
00223 return Max_Speed;
00224
00225 }
00226
00227
00236 unsigned char kgripper_Arm_Get_Search_Limit( knet_dev_t * dev )
00237 {
00238 unsigned char Search_Limit;
00239 knet_read8( dev , ARM_SEARCH_LIMIT , &Search_Limit );
00240
00241 return Search_Limit;
00242
00243 }
00244
00245
00246
00254 void kgripper_Arm_Set_Order( knet_dev_t * dev, unsigned short Order )
00255 {
00256 knet_set_order( dev , KGRIPPER_ORDER_MASK );
00257 knet_write16( dev , ARM_ORDER , Order );
00258
00259
00260
00261 }
00262
00263
00271 void kgripper_Arm_Set_Max_Speed( knet_dev_t * dev, unsigned char Max_Speed )
00272 {
00273 if(Max_Speed > 15)
00274 Max_Speed = 15;
00275 knet_write8( dev , ARM_MAX_SPEED , Max_Speed );
00276
00277 }
00278
00279
00287 void kgripper_Arm_Set_Search_Limit( knet_dev_t * dev, unsigned char Search_Limit )
00288 {
00289 if(Search_Limit > 1)
00290 Search_Limit = 1;
00291 knet_write8( dev , ARM_SEARCH_LIMIT , Search_Limit );
00292
00293 }
00294
00295
00296
00297
00298
00307 unsigned char kgripper_Gripper_Get_Version( knet_dev_t * dev )
00308 {
00309 unsigned char OSVersion;
00310 knet_read8( dev , GRIPPER_VERSION , &OSVersion );
00311
00312 return OSVersion;
00313
00314 }
00315
00316
00327 unsigned char kgripper_Gripper_Get_Position( knet_dev_t * dev )
00328 {
00329 unsigned char Position;
00330 knet_read8( dev , GRIPPER_POSITION , &Position );
00331
00332 return Position;
00333
00334 }
00335
00336
00344 unsigned char kgripper_Gripper_Get_Order( knet_dev_t * dev )
00345 {
00346 unsigned char Order;
00347 knet_read8( dev , GRIPPER_ORDER , &Order );
00348
00349 return Order;
00350
00351 }
00352
00353
00354
00363 unsigned char kgripper_Gripper_Get_Speed( knet_dev_t * dev )
00364 {
00365 unsigned char Speed;
00366 knet_read8( dev , GRIPPER_SPEED , &Speed );
00367
00368 return Speed;
00369
00370 }
00371
00372
00373
00381 unsigned short kgripper_Gripper_Get_Current( knet_dev_t * dev )
00382 {
00383 unsigned short Current;
00384 knet_read16( dev , GRIPPER_CURRENT , &Current );
00385
00386 return Current;
00387
00388 }
00389
00390
00398 unsigned short kgripper_Gripper_Get_Torque( knet_dev_t * dev )
00399 {
00400 unsigned short Torque;
00401 knet_read16( dev , GRIPPER_TORQUE , &Torque );
00402
00403 return Torque;
00404
00405 }
00406
00407
00415 unsigned short kgripper_Gripper_Get_Resistivity( knet_dev_t * dev )
00416 {
00417 unsigned short Resistivity;
00418 knet_read16( dev , GRIPPER_RESISTIVITY , &Resistivity );
00419
00420 return Resistivity;
00421
00422 }
00423
00424
00434 void kgripper_Gripper_Get_Ambiant_IR_Light( knet_dev_t * dev ,
00435 unsigned short * Amb_IR_Left ,
00436 unsigned short * Amb_IR_Right )
00437 {
00438
00439 if (Amb_IR_Left != NULL )
00440 knet_read16( dev , GRIPPER_AMB_IR_LEFT , Amb_IR_Left );
00441
00442 if ( Amb_IR_Right != NULL )
00443 knet_read16( dev , GRIPPER_AMB_IR_RIGHT , Amb_IR_Right );
00444 }
00445
00446
00456 void kgripper_Gripper_Get_Distance_Sensors( knet_dev_t * dev ,
00457 unsigned short * Dist_IR_Left ,
00458 unsigned short * Dist_IR_Right )
00459 {
00460
00461 if (Dist_IR_Left != NULL )
00462 knet_read16( dev , GRIPPER_DIST_IR_LEFT , Dist_IR_Left );
00463
00464 if ( Dist_IR_Right != NULL )
00465 knet_read16( dev , GRIPPER_DIST_IR_RIGHT , Dist_IR_Right );
00466 }
00467
00468
00469
00478 unsigned char kgripper_Gripper_OnTarget( knet_dev_t * dev )
00479 {
00480 unsigned char OnTarget;
00481 knet_read8( dev , GRIPPER_ON_TARGET , &OnTarget );
00482
00483 return OnTarget;
00484
00485 }
00486
00487
00496 unsigned char kgripper_Gripper_Object_Detected( knet_dev_t * dev )
00497 {
00498 unsigned char Object;
00499 knet_read8( dev , GRIPPER_OPTICAL_BARR , &Object );
00500
00501 return Object;
00502
00503 }
00504
00505
00506
00515 unsigned char kgripper_Gripper_Get_Search_Limit( knet_dev_t * dev )
00516 {
00517 unsigned char Search_Limit;
00518 knet_read8( dev , GRIPPER_SEARCH_LIMIT , &Search_Limit );
00519
00520 return Search_Limit;
00521
00522 }
00523
00532 unsigned char kgripper_Gripper_Get_Limits( knet_dev_t * dev )
00533 {
00534 unsigned char Max_Position;
00535
00536 knet_read8( dev , GRIPPER_MAX_POSITION , &Max_Position );
00537
00538 return Max_Position;
00539 }
00540
00541
00542
00550 void kgripper_Gripper_Set_Order( knet_dev_t * dev, unsigned char Order )
00551 {
00552 knet_write8( dev , GRIPPER_ORDER , Order );
00553 }
00554
00555
00563 void kgripper_Gripper_Set_Torque( knet_dev_t * dev, unsigned short Torque )
00564 {
00565 knet_set_order( dev , KGRIPPER_ORDER_MASK );
00566 knet_write16( dev , GRIPPER_TORQUE , Torque );
00567
00568 }
00569
00570
00578 void kgripper_GripperSet_Search_Limit( knet_dev_t * dev, unsigned char Search_Limit )
00579 {
00580 if(Search_Limit > 1)
00581 Search_Limit = 1;
00582 knet_write8( dev , GRIPPER_SEARCH_LIMIT , Search_Limit );
00583
00584 }
00585
00586
00587
00588
00589