00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074 #include "korebot.h"
00075
00094
00102 void kmot_GetFWVersion( knet_dev_t * dev ,
00103 unsigned int * version )
00104 {
00105 unsigned char val;
00106
00107 knet_read8( dev , MOT_FWVersion, &val );
00108 *version = val;
00109 }
00110
00111
00141 void kmot_GetStatus( knet_dev_t * dev ,
00142 unsigned char * status ,
00143 unsigned char * error )
00144 {
00145 if ( status != NULL )
00146 knet_read8( dev , MOT_StatusFlags , status );
00147
00148 if ( error != NULL )
00149 knet_read8( dev , MOT_ErrorFlags , error );
00150 }
00151
00152
00153
00154
00158 void kmot_GetSpeedMultiplier( knet_dev_t * dev ,
00159 unsigned short *mult )
00160 {
00161 unsigned char lsb, msb;
00162 unsigned char *plsb, *pmsb;
00163
00164
00165 plsb = &lsb;
00166 pmsb = &msb;
00167
00168 if ( mult != NULL )
00169 {
00170 knet_read8( dev , MOT_SpeedMultL , plsb );
00171 knet_read8( dev , MOT_SpeedMultH , pmsb );
00172
00173 *mult = (msb<<8) | lsb;
00174
00175 printf("kmot_GetSpeedMultiplier : mult = %x, lsb = %x, msb = %x\r\n",
00176 *mult, lsb, msb);
00177
00178 }
00179 }
00180
00181
00182
00183
00218 void kmot_GetOptions( knet_dev_t * dev ,
00219 unsigned char * software,
00220 unsigned char * hardware)
00221 {
00222 if ( hardware != NULL )
00223 knet_read8( dev , MOT_HWOptions, hardware);
00224
00225 if ( software != NULL )
00226 knet_read8( dev , MOT_SWOptions, software);
00227 }
00228
00229
00241 void kmot_ConfigurePID( knet_dev_t * dev ,
00242 int regtype ,
00243 int16_t Kp , int16_t Kd , int16_t Ki )
00244 {
00245 unsigned char regs[3];
00246
00247 switch(regtype) {
00248
00249
00250 case kMotRegPos:
00251 regs[0] = MOT_KpPosL;
00252 regs[1] = MOT_KdPosL;
00253 regs[2] = MOT_KiPosL;
00254 break;
00255
00256 case kMotRegPosProfile:
00257 case kMotRegSpeed:
00258 case kMotRegSpeedProfile:
00259
00260 regs[0] = MOT_KpSpeedL;
00261 regs[1] = MOT_KdSpeedL;
00262 regs[2] = MOT_KiSpeedL;
00263 break;
00264
00265 case kMotRegTorque:
00266 regs[0] = MOT_KpTorqueL;
00267 regs[1] = MOT_KdTorqueL;
00268 regs[2] = MOT_KiTorqueL;
00269 break;
00270 }
00271
00272 knet_set_order( dev , KMOT_ORDER_MASK );
00273
00274 knet_write16( dev , regs[0] , Kp );
00275 knet_write16( dev , regs[1] , Kd );
00276 knet_write16( dev , regs[2] , Ki );
00277 }
00278
00279
00292 void kmot_SetPointSource( knet_dev_t * dev ,
00293 int regtype ,
00294 int wavetype ,
00295 int period ,
00296 int amplitude ,
00297 int offset )
00298 {
00299 knet_write8( dev , MOT_SetPointSource , wavetype );
00300 knet_write8( dev , MOT_IntGenPeriod , period );
00301 knet_write8( dev , MOT_IntGenAmplitude , amplitude );
00302 knet_write8( dev , MOT_IntGenOffset , offset );
00303 knet_write8( dev , MOT_Mode , 1 );
00304 knet_write8( dev , MOT_ControlTyp , regtype );
00305 }
00306
00307
00316 long kmot_GetMeasure( knet_dev_t * dev , int regtype )
00317 {
00318 short s;
00319 long m = 0; int rc;
00320
00321 knet_set_order( dev , KMOT_ORDER_MASK );
00322
00323 switch(regtype) {
00324
00325 default:
00326 case kMotRegOpenLoop:
00327 m = 0;
00328 break;
00329
00330 case kMotRegPos:
00331 case kMotRegPosProfile:
00332 rc=knet_read32( dev , MOT_PositionLL , &m );
00333 break;
00334
00335 #ifdef NEW_KMOT_FIRMWARE
00336 case kMotRegSpeed:
00337 case kMotRegSpeedProfile:
00338 rc=knet_read32( dev , MOT_SpeedLL , &m );
00339 break;
00340 #else
00341 case kMotRegSpeed:
00342 case kMotRegSpeedProfile:
00343 knet_read16( dev , MOT_SpeedLL , &s );
00344 m = (long)s;
00345 break;
00346 #endif
00347 case kMotRegTorque:
00348 knet_read16( dev , MOT_TorqueL , &s );
00349 m = (long)s;
00350 break;
00351 }
00352
00353 return m;
00354 }
00355
00356
00365 void kmot_SetPosition( knet_dev_t * dev , long position )
00366 {
00367 knet_set_order( dev , KMOT_ORDER_MASK );
00368 knet_write32( dev , MOT_PositionLL , position );
00369 }
00370
00371
00383 void kmot_SetSpeedProfile( knet_dev_t * dev ,
00384 int maxspeed ,
00385 int acceleration )
00386 {
00387 knet_set_order( dev , KMOT_ORDER_MASK );
00388 knet_write16( dev , MOT_MaxSpeedL , maxspeed );
00389 knet_write8( dev , MOT_Acceleration , acceleration );
00390 }
00391
00392
00402 void kmot_SetMinSpeed( knet_dev_t * dev ,
00403 int minspeed )
00404 {
00405 knet_set_order( dev , KMOT_ORDER_MASK );
00406 knet_write16( dev , MOT_MinSpeedL , minspeed );
00407 }
00408
00409
00410
00411
00424 void kmot_SetLimits( knet_dev_t *dev ,
00425 int regtype ,
00426 long softStopMin , long softStopMax )
00427 {
00428 unsigned short v;
00429
00430 knet_set_order( dev , KMOT_ORDER_MASK );
00431
00432 switch(regtype) {
00433
00434 case kMotRegPos:
00435 case kMotRegPosProfile:
00436 knet_write32( dev , MOT_SoftStopMinLL , softStopMin );
00437 knet_write32( dev , MOT_SoftStopMaxLL , softStopMax );
00438 break;
00439
00440 case kMotRegTorque:
00441
00442 v = (unsigned short)softStopMax;
00443 knet_write16( dev , MOT_SWCurrentLimitL , v );
00444 break;
00445
00446 default:
00447 break;
00448 }
00449 }
00450
00451
00474 int kmot_SearchLimits( knet_dev_t * dev ,
00475 int8_t blockedTime ,
00476 int32_t setPoint ,
00477 int32_t * minpos ,
00478 int32_t * maxpos ,
00479 unsigned int timeout )
00480 {
00481 kb_time_t t;
00482 int ok;
00483
00484 knet_set_order( dev , KMOT_ORDER_MASK );
00485
00486 knet_write8( dev , MOT_BlockedTime , blockedTime );
00487 knet_write8( dev , MOT_ControlTyp , 0 );
00488
00489 knet_write32( dev , MOT_SetPointLL , setPoint );
00490
00491 knet_write8( dev , MOT_Mode , kMotModeSearchLimit );
00492
00493 t = kb_getTime() + timeout;
00494
00495 ok = 0;
00496 do {
00497 if ( knet_read( dev , MOT_Mode ) == 0 ) {
00498 ok = 1;
00499 break;
00500 }
00501 usleep( 10000 );
00502 }
00503 while ( kb_getTime() < t );
00504
00505 if (!ok) return -1;
00506
00507 knet_read32( dev , MOT_SoftStopMinLL , (unsigned long *)minpos );
00508 knet_read32( dev , MOT_SoftStopMaxLL , (unsigned long *)maxpos );
00509
00510 return 0;
00511 }
00512
00513
00555 void kmot_SetOptions( knet_dev_t *dev , int hwOptions , int swOptions )
00556 {
00557 knet_write8( dev , MOT_HWOptions , hwOptions );
00558 knet_write8( dev , MOT_SWOptions , swOptions );
00559 }
00560
00561
00569 void kmot_SetPoint( knet_dev_t * dev , int regtype , long setPoint )
00570 {
00571
00572
00573 knet_set_order( dev , KMOT_ORDER_MASK );
00574
00575 if ( knet_read( dev , MOT_Mode ) != kMotModeNormal ) {
00576 knet_write8( dev , MOT_Mode , kMotModeNormal );
00577 }
00578
00579
00580
00581
00582
00583
00584
00585
00586
00587
00588
00589
00590 knet_write8( dev , MOT_ControlTyp , regtype );
00591 knet_write32( dev , MOT_SetPointLL , setPoint );
00592 knet_write8( dev , MOT_SetPointSource , kMotPtSrcExtI2C );
00593
00594
00595
00596
00597
00598
00599 }
00600
00601
00609 void kmot_SaveConfig( knet_dev_t * dev )
00610 {
00611 knet_write8( dev , MOT_Mode , 0x55 );
00612 knet_write8( dev , MOT_Mode , 0xAA );
00613 knet_write8( dev , MOT_Mode , kMotModeSaveE2PROM );
00614 }
00615
00616
00623 void kmot_ResetError( knet_dev_t * dev )
00624 {
00625 knet_write8( dev , MOT_ErrorFlags , 0 );
00626 }
00627
00628
00635 void kmot_SetBlockedTime( knet_dev_t * dev , int time )
00636 {
00637 knet_write8( dev , MOT_BlockedTime , time );
00638 }
00639
00640
00647 void kmot_SetSampleTime( knet_dev_t * dev , int sample )
00648 {
00649 knet_set_order( dev , KMOT_ORDER_MASK );
00650 knet_write8( dev , MOT_SampleTimeH , (sample & 0xFF00) >> 8 );
00651 knet_write8( dev , MOT_SampleTimeL , (sample & 0x00FF));
00652 }
00653
00654
00662 void kmot_SetFilterOrder( knet_dev_t * dev , int order )
00663 {
00664 knet_write8( dev , MOT_Filter, order );
00665 }
00666
00667
00674 void kmot_SetMode( knet_dev_t * dev , int mode )
00675 {
00676 knet_write8( dev , MOT_Mode , mode );
00677 }
00678
00679
00680
00687 void kmot_SetPrescale( knet_dev_t * dev , int mode )
00688 {
00689 knet_write8( dev , MOT_Prescale , mode );
00690 }
00691
00692
00693
00700 void kmot_SetSpeedMultiplier( knet_dev_t * dev , int mode )
00701 {
00702 short value;
00703 char lsb, msb;
00704
00705 value = mode;
00706
00707 lsb = value & 0xFF;
00708 msb = (value >> 8) & 0xFF;
00709
00710 printf("kmot_SetSpeedMultiplier : mode = %x, value = %x, lsb = %x, msb = %x\r\n",
00711 mode, value, lsb, msb);
00712
00713 knet_write8( dev , MOT_SpeedMultL , lsb );
00714 knet_write8( dev , MOT_SpeedMultH , msb );
00715
00716 }
00717
00718
00719
00720
00721
00722
00730 void kmot_SetVelocityPrescale( knet_dev_t * dev, int mode )
00731 {
00732 knet_write8( dev , MOT_VelocityPrescaler , mode );
00733 }
00734
00735
00742 void kmot_SetMargin( knet_dev_t *dev , int margin )
00743 {
00744 knet_write8( dev , MOT_NearTargetMargin , margin );
00745 }