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