RoboClaw.cpp

00001 #include "RoboClaw.h"
00002 #include <stdarg.h>
00003 
00004 #define MAXTRY 1
00005 #define SetDWORDval(arg) (uint8_t)(arg>>24),(uint8_t)(arg>>16),(uint8_t)(arg>>8),(uint8_t)arg
00006 #define SetWORDval(arg) (uint8_t)(arg>>8),(uint8_t)arg
00007 
00008 RoboClaw::RoboClaw(uint8_t adr, int baudrate, PinName rx, PinName tx) : _roboclaw(rx, tx){
00009     _roboclaw.baud(baudrate);
00010     address = adr;
00011 }
00012 
00013 void RoboClaw::crc_clear(){
00014     crc = 0;
00015 }
00016 
00017 void RoboClaw::crc_update (uint8_t data){
00018     int i;
00019     crc = crc ^ ((uint16_t)data << 8);
00020     for (i=0; i<8; i++) {
00021         if (crc & 0x8000)
00022             crc = (crc << 1) ^ 0x1021;
00023         else
00024             crc <<= 1;
00025     }
00026 }
00027 
00028 uint16_t RoboClaw::crc_get(){
00029     return crc;
00030 }
00031 
00032 void RoboClaw::write_n(uint8_t cnt, ... ){
00033     //uint8_t retry = MAXTRY;
00034     //do {
00035         crc_clear();
00036         va_list marker;
00037         va_start( marker, cnt );
00038         for(uint8_t index=0; index<cnt; index++) {
00039             uint8_t data = va_arg(marker, unsigned int);
00040             crc_update(data);
00041             _roboclaw.putc(data);
00042         }
00043         va_end( marker );
00044         uint16_t crc = crc_get();
00045         _roboclaw.putc(crc>>8);
00046         _roboclaw.putc(crc);
00047     //} while(_roboclaw.getc() != 0xFF);
00048 }
00049 
00050 void RoboClaw::write_(uint8_t command, uint8_t data, bool reading, bool crcon){
00051     _roboclaw.putc(address);
00052     _roboclaw.putc(command);
00053 
00054     if(reading == false) {
00055         if(crcon == true) {
00056             uint8_t packet[2] = {address, command};
00057             uint16_t checksum = crc16(packet,2);
00058             _roboclaw.putc(checksum>>8);
00059             _roboclaw.putc(checksum);
00060         } else {
00061             uint8_t packet[3] = {address, command, data};
00062             uint16_t checksum = crc16(packet,3);
00063             _roboclaw.putc(data);
00064             _roboclaw.putc(checksum>>8);
00065             _roboclaw.putc(checksum);
00066         }
00067     }
00068 }
00069 
00070 uint16_t RoboClaw::crc16(uint8_t *packet, int nBytes){
00071     uint16_t crc_;
00072     for (int byte = 0; byte < nBytes; byte++) {
00073         crc_ = crc_ ^ ((uint16_t)packet[byte] << 8);
00074         for (uint8_t bit = 0; bit < 8; bit++) {
00075             if (crc_ & 0x8000) {
00076                 crc_ = (crc_ << 1) ^ 0x1021;
00077             } else {
00078                 crc_ = crc_ << 1;
00079             }
00080         }
00081     }
00082     return crc_;
00083 }
00084 
00085 uint8_t RoboClaw::read_(void){
00086     return(_roboclaw.getc());
00087 }
00088 
00089 void RoboClaw::ForwardM1(int speed){
00090     write_(M1FORWARD,speed,false,false);
00091 }
00092 
00093 void RoboClaw::BackwardM1(int speed){
00094     write_(M1BACKWARD,speed,false,false);
00095 }
00096 
00097 void RoboClaw::ForwardM2(int speed){
00098     write_(M2FORWARD,speed,false,false);
00099 }
00100 
00101 void RoboClaw::BackwardM2(int speed){
00102     write_(M2BACKWARD,speed,false,false);
00103 }
00104 
00105 void RoboClaw::Forward(int speed){
00106     write_(MIXEDFORWARD,speed,false,false);
00107 }
00108 
00109 void RoboClaw::Backward(int speed){
00110     write_(MIXEDBACKWARD,speed,false,false);
00111 }
00112 
00113 void RoboClaw::ReadFirm(){
00114     write_(GETVERSION,0x00,true,false);
00115 }
00116 
00117 int32_t RoboClaw::ReadEncM1(){
00118     int32_t enc1;
00119     uint16_t read_byte[7];
00120     write_n(2,address,GETM1ENC);
00121 
00122     read_byte[0] = (uint16_t)_roboclaw.getc();
00123     read_byte[1] = (uint16_t)_roboclaw.getc();
00124     read_byte[2] = (uint16_t)_roboclaw.getc();
00125     read_byte[3] = (uint16_t)_roboclaw.getc();
00126     read_byte[4] = (uint16_t)_roboclaw.getc();
00127     read_byte[5] = (uint16_t)_roboclaw.getc();
00128     read_byte[6] = (uint16_t)_roboclaw.getc();
00129 
00130     enc1 = read_byte[1]<<24;
00131     enc1 |= read_byte[2]<<16;
00132     enc1 |= read_byte[3]<<8;
00133     enc1 |= read_byte[4];
00134 
00135     return enc1;
00136 }
00137 
00138 int32_t RoboClaw::ReadEncM2(){
00139     int32_t enc2;
00140     uint16_t read_byte2[7];
00141     write_(GETM2ENC,0x00, true,false);
00142 
00143     read_byte2[0] = (uint16_t)_roboclaw.getc();
00144     read_byte2[1] = (uint16_t)_roboclaw.getc();
00145     read_byte2[2] = (uint16_t)_roboclaw.getc();
00146     read_byte2[3] = (uint16_t)_roboclaw.getc();
00147     read_byte2[4] = (uint16_t)_roboclaw.getc();
00148     read_byte2[5] = (uint16_t)_roboclaw.getc();
00149     read_byte2[6] = (uint16_t)_roboclaw.getc();
00150 
00151     enc2 = read_byte2[1]<<24;
00152     enc2 |= read_byte2[2]<<16;
00153     enc2 |= read_byte2[3]<<8;
00154     enc2 |= read_byte2[4];
00155 
00156     return enc2;
00157 }
00158 
00159 int32_t RoboClaw::ReadSpeedM1(){
00160     int32_t speed1;
00161     uint16_t read_byte[7];
00162     write_n(2,address,GETM1SPEED);
00163 
00164     read_byte[0] = (uint16_t)_roboclaw.getc();
00165     read_byte[1] = (uint16_t)_roboclaw.getc();
00166     read_byte[2] = (uint16_t)_roboclaw.getc();
00167     read_byte[3] = (uint16_t)_roboclaw.getc();
00168     read_byte[4] = (uint16_t)_roboclaw.getc();
00169     read_byte[5] = (uint16_t)_roboclaw.getc();
00170     read_byte[6] = (uint16_t)_roboclaw.getc();
00171 
00172     speed1 = read_byte[1]<<24;
00173     speed1 |= read_byte[2]<<16;
00174     speed1 |= read_byte[3]<<8;
00175     speed1 |= read_byte[4];
00176 
00177     return speed1;
00178 }
00179 
00180 int32_t RoboClaw::ReadSpeedM2(){
00181     int32_t speed2;
00182     uint16_t read_byte2[7];
00183     write_n(2,address,GETM2SPEED);
00184 
00185     read_byte2[0] = (uint16_t)_roboclaw.getc();
00186     read_byte2[1] = (uint16_t)_roboclaw.getc();
00187     read_byte2[2] = (uint16_t)_roboclaw.getc();
00188     read_byte2[3] = (uint16_t)_roboclaw.getc();
00189     read_byte2[4] = (uint16_t)_roboclaw.getc();
00190     read_byte2[5] = (uint16_t)_roboclaw.getc();
00191     read_byte2[6] = (uint16_t)_roboclaw.getc();
00192 
00193     speed2 = read_byte2[1]<<24;
00194     speed2 |= read_byte2[2]<<16;
00195     speed2 |= read_byte2[3]<<8;
00196     speed2 |= read_byte2[4];
00197 
00198     return speed2;
00199 }
00200 
00201 void RoboClaw::ResetEnc(){
00202     write_n(2,address,RESETENC);
00203 }
00204 
00205 void RoboClaw::SpeedM1(int32_t speed){
00206     write_n(6,address,M1SPEED,SetDWORDval(speed));
00207 }
00208 
00209 void RoboClaw::SpeedM2(int32_t speed){
00210     write_n(6,address,M2SPEED,SetDWORDval(speed));
00211 }
00212 
00213 void RoboClaw::SpeedAccelM1(int32_t accel, int32_t speed){
00214     write_n(10,address,M1SPEEDACCEL,SetDWORDval(accel),SetDWORDval(speed));
00215 }
00216 
00217 void RoboClaw::SpeedAccelM2(int32_t accel, int32_t speed){
00218     write_n(10,address,M2SPEEDACCEL,SetDWORDval(accel),SetDWORDval(speed));
00219 }
00220 
00221 void RoboClaw::SpeedAccelM1M2(int32_t accel, int32_t speed1, int32_t speed2){
00222     write_n(14,address,MIXEDSPEEDACCEL,SetDWORDval(accel),SetDWORDval(speed1),SetDWORDval(speed2));
00223 }
00224 
00225 void RoboClaw::SpeedDistanceM1(int32_t speed, uint32_t distance, uint8_t buffer){
00226     write_n(11,address,M1SPEEDDIST,SetDWORDval(speed),SetDWORDval(distance),buffer);
00227 }
00228 
00229 void RoboClaw::SpeedDistanceM2(int32_t speed, uint32_t distance, uint8_t buffer){
00230     write_n(11,address,M2SPEEDDIST,SetDWORDval(speed),SetDWORDval(distance),buffer);
00231 }
00232 
00233 void RoboClaw::SpeedAccelDistanceM1(int32_t accel, int32_t speed, uint32_t distance, uint8_t buffer){
00234     write_n(15,address,M1SPEEDACCELDIST,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(distance),buffer);
00235 }
00236 
00237 void RoboClaw::SpeedAccelDistanceM2(int32_t accel, int32_t speed, uint32_t distance, uint8_t buffer){
00238     write_n(15,address,M2SPEEDACCELDIST,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(distance),buffer);
00239 }
00240 
00241 void RoboClaw::SpeedAccelDeccelPositionM1(uint32_t accel, int32_t speed, uint32_t deccel, int32_t position, uint8_t flag){
00242     write_n(19,address,M1SPEEDACCELDECCELPOS,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(deccel),SetDWORDval(position),flag);
00243 }
00244 
00245 void RoboClaw::SpeedAccelDeccelPositionM2(uint32_t accel, int32_t speed, uint32_t deccel, int32_t position, uint8_t flag){
00246     write_n(19,address,M2SPEEDACCELDECCELPOS,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(deccel),SetDWORDval(position),flag);
00247 }
00248 
00249 void RoboClaw::SpeedAccelDeccelPositionM1M2(uint32_t accel1,uint32_t speed1,uint32_t deccel1, int32_t position1,uint32_t accel2,uint32_t speed2,uint32_t deccel2, int32_t position2,uint8_t flag){
00250     write_n(35,address,MIXEDSPEEDACCELDECCELPOS,SetDWORDval(accel1),SetDWORDval(speed1),SetDWORDval(deccel1),SetDWORDval(position1),SetDWORDval(accel2),SetDWORDval(speed2),SetDWORDval(deccel2),SetDWORDval(position2),flag);
00251 }
00252