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, double _read_timeout) : _roboclaw(rx, tx){
00009     _roboclaw.set_baud(baudrate);//_roboclaw.baud(baudrate);
00010        
00011     rx_in = 0;
00012     address = adr;
00013     //readTime = 0.0;
00014     //readTimer.start();
00015     read_timeout = _read_timeout;
00016     
00017 }
00018 
00019 void RoboClaw::crc_clear(){
00020     crc = 0;
00021 }
00022 
00023 
00024 void RoboClaw::flushSerialBuffer(void) { 
00025     char char1[1];
00026      
00027     while (_roboclaw.readable()) { 
00028         _roboclaw.read_timeout(char1,1,read_timeout); 
00029     } 
00030 return; }
00031 
00032 void RoboClaw::crc_update (uint8_t data){
00033     int i;
00034     crc = crc ^ ((uint16_t)data << 8);
00035     for (i=0; i<8; i++) {
00036         if (crc & 0x8000)
00037             crc = (crc << 1) ^ 0x1021;
00038         else
00039             crc <<= 1;
00040     }
00041 }
00042 
00043 uint16_t RoboClaw::crc_get(){
00044     return crc;
00045 }
00046 
00047 uint16_t RoboClaw::crc16(uint8_t *packet, int nBytes){
00048     uint16_t crc_;
00049     for (int byte = 0; byte < nBytes; byte++) {
00050         crc_ = crc_ ^ ((uint16_t)packet[byte] << 8);
00051         for (uint8_t bit = 0; bit < 8; bit++) {
00052             if (crc_ & 0x8000) {
00053                 crc_ = (crc_ << 1) ^ 0x1021;
00054             } else {
00055                 crc_ = crc_ << 1;
00056             }
00057         }
00058     }
00059     return crc_;
00060 }
00061 
00062 void RoboClaw::write_n(uint8_t cnt, ... ){
00063     uint8_t count = 0;
00064     char dat[1];
00065     char r_dat[1];
00066     r_dat[0] = ' ';
00067     char crc_[2];
00068     do {
00069         _roboclaw.flush();
00070 
00071         crc_clear();
00072         va_list marker;
00073         va_start( marker, cnt );
00074         for(uint8_t index=0; index<cnt; index++) {
00075             uint8_t data = va_arg(marker, unsigned int);
00076             dat[0] = data;
00077             crc_update(data);
00078             _roboclaw.write(dat,1);
00079                    }
00080         va_end( marker );
00081         uint16_t crc = crc_get();
00082         crc_[0]=crc>>8;
00083         crc_[1]=crc;
00084         _roboclaw.write(crc_,2);        
00085         _roboclaw.read_timeout(r_dat,1,read_timeout);
00086         
00087          count ++;
00088          
00089         }while((uint16_t)r_dat[0] != 0xFF&& count <=MAXTRY);
00090 
00091 }
00092 
00093 void RoboClaw::write_(uint8_t command, uint8_t data, bool reading, bool crcon){
00094    // _roboclaw.putc(address);
00095    // _roboclaw.putc(command);
00096 
00097     if(reading == false) {
00098         if(crcon == true) {
00099             uint8_t packet[2] = {address, command};
00100             uint16_t checksum = crc16(packet,2);
00101            // _roboclaw.putc(checksum>>8);
00102            // _roboclaw.putc(checksum);
00103         } else {
00104             uint8_t packet[3] = {address, command, data};
00105             uint16_t checksum = crc16(packet,3);
00106            // _roboclaw.putc(data);
00107            // _roboclaw.putc(checksum>>8);
00108            // _roboclaw.putc(checksum);
00109         }
00110     }
00111 }
00112 
00113 bool RoboClaw::read_n(uint8_t address, uint8_t cmd, int n_byte, int32_t &value){
00114     uint16_t read_byte[n_byte];
00115     uint16_t received_crc;
00116     int dataRead=0;
00117     char tx_buffer[2];
00118     
00119     tx_buffer[0] = address;
00120     tx_buffer[1] = cmd;    
00121 
00122     rx_in = 0;    
00123     crc_clear();
00124 
00125     _roboclaw.flush();
00126     _roboclaw.write(tx_buffer,2);
00127     
00128     crc_update(address);
00129     crc_update(cmd);
00130     
00131     dataRead = _roboclaw.read_timeout(rx_buffer,7,read_timeout);
00132     //printf("d: %d\r\n", dataRead);
00133     //printf("add: %x,   cmd: %x,   0: %x,   1: %x,   2: %x,   3: %x,   4: %x,   5: %x,   6: %x \r\n",(uint16_t)tx_buffer[0],(uint16_t)tx_buffer[1],(uint16_t)rx_buffer[0],(uint16_t)rx_buffer[1],(uint16_t)rx_buffer[2],(uint16_t)rx_buffer[3],(uint16_t)rx_buffer[4],(uint16_t)rx_buffer[5],(uint16_t)rx_buffer[6]); 
00134     //printf("dopo del read\r\n");       
00135     for(int i = 0; i<=n_byte-3; i++){
00136         crc_update((uint16_t)rx_buffer[i]);
00137     }
00138     
00139     received_crc = (uint16_t)rx_buffer[n_byte-2]<<8;
00140     received_crc |= (uint16_t)rx_buffer[n_byte-1];
00141     
00142     if(received_crc == crc_get()){
00143         value = (uint16_t)rx_buffer[0]<<24;
00144         value |= (uint16_t)rx_buffer[1]<<16;
00145         value |= (uint16_t)rx_buffer[2]<<8;
00146         value |= (uint16_t)rx_buffer[3];
00147         return true;
00148     }else{
00149         //printf("crc: %x \r\n", crc_get());
00150         //printf("received crc: %x \r\n", received_crc);
00151         printf("err \r\n");
00152 
00153 
00154         return false;        
00155     }
00156 }
00157 
00158 
00159 void RoboClaw::ForwardM1(int speed){
00160     write_(M1FORWARD,speed,false,false);
00161 }
00162 
00163 void RoboClaw::BackwardM1(int speed){
00164     write_(M1BACKWARD,speed,false,false);
00165 }
00166 
00167 void RoboClaw::ForwardM2(int speed){
00168     write_(M2FORWARD,speed,false,false);
00169 }
00170 
00171 void RoboClaw::BackwardM2(int speed){
00172     write_(M2BACKWARD,speed,false,false);
00173 }
00174 
00175 void RoboClaw::Forward(int speed){
00176     write_(MIXEDFORWARD,speed,false,false);
00177 }
00178 
00179 void RoboClaw::Backward(int speed){
00180     write_(MIXEDBACKWARD,speed,false,false);
00181 }
00182 
00183 void RoboClaw::ReadFirm(){
00184     write_(GETVERSION,0x00,true,false);
00185 }
00186 
00187 bool RoboClaw::ReadEncM1(int32_t &encPulse){
00188     int32_t value=0;
00189     if (read_n(address, GETM1ENC, 7, value) == true){
00190         encPulse = value;
00191         return true;
00192     }else{
00193         return false;
00194     }
00195 }
00196 
00197 bool RoboClaw::ReadEncM2(int32_t &encPulse){
00198     int32_t value=0;
00199     if (read_n(address, GETM2ENC, 7, value) == true){
00200         encPulse = value;
00201         return true;
00202     }else{
00203         return false;
00204     }
00205 }
00206 
00207 bool RoboClaw::ReadSpeedM1(int32_t &speed){
00208     int32_t value=0;
00209     if (read_n(address, GETM1SPEED, 7, value) == true){
00210         speed = value;
00211         return true;
00212     }else{
00213         return false;
00214     }
00215 }
00216 
00217 bool RoboClaw::ReadSpeedM2(int32_t &speed){
00218     int32_t value=0;
00219     if (read_n(address, GETM2SPEED, 7, value) == true){
00220         speed = value;
00221         return true;
00222     }else{
00223         return false;
00224     }
00225 }
00226 
00227 
00228 bool RoboClaw::ReadCurrentM1M2(int32_t& currentM1, int32_t& currentM2){
00229     int32_t value=0;
00230     if (read_n(address, GETCURRENTS, 6, value) == true){
00231         currentM1 = value>>16;
00232         currentM2 = value&0xFFFF;
00233         return true;
00234     }else{
00235         return false;
00236     }
00237 
00238 }
00239 
00240 void RoboClaw::ResetEnc(){
00241     write_n(2,address,RESETENC);
00242 }
00243 
00244 void RoboClaw::SpeedM1(int32_t speed){
00245     write_n(6,address,M1SPEED,SetDWORDval(speed));
00246 }
00247 
00248 void RoboClaw::SpeedM2(int32_t speed){
00249     write_n(6,address,M2SPEED,SetDWORDval(speed));
00250 }
00251 
00252 void RoboClaw::SpeedAccelM1(int32_t accel, int32_t speed){
00253     write_n(10,address,M1SPEEDACCEL,SetDWORDval(accel),SetDWORDval(speed));
00254 }
00255 
00256 void RoboClaw::SpeedAccelM2(int32_t accel, int32_t speed){
00257     write_n(10,address,M2SPEEDACCEL,SetDWORDval(accel),SetDWORDval(speed));
00258 }
00259 
00260 void RoboClaw::SpeedAccelM1M2(int32_t accel, int32_t speed1, int32_t speed2){
00261     write_n(14,address,MIXEDSPEEDACCEL,SetDWORDval(accel),SetDWORDval(speed1),SetDWORDval(speed2));
00262 }
00263 
00264 void RoboClaw::SpeedDistanceM1(int32_t speed, uint32_t distance, uint8_t buffer){
00265     write_n(11,address,M1SPEEDDIST,SetDWORDval(speed),SetDWORDval(distance),buffer);
00266 }
00267 
00268 void RoboClaw::SpeedDistanceM2(int32_t speed, uint32_t distance, uint8_t buffer){
00269     write_n(11,address,M2SPEEDDIST,SetDWORDval(speed),SetDWORDval(distance),buffer);
00270 }
00271 
00272 void RoboClaw::SpeedAccelDistanceM1(int32_t accel, int32_t speed, uint32_t distance, uint8_t buffer){
00273     write_n(15,address,M1SPEEDACCELDIST,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(distance),buffer);
00274 }
00275 
00276 void RoboClaw::SpeedAccelDistanceM2(int32_t accel, int32_t speed, uint32_t distance, uint8_t buffer){
00277     write_n(15,address,M2SPEEDACCELDIST,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(distance),buffer);
00278 }
00279 
00280 void RoboClaw::SpeedAccelDeccelPositionM1(uint32_t accel, int32_t speed, uint32_t deccel, int32_t position, uint8_t flag){
00281     write_n(19,address,M1SPEEDACCELDECCELPOS,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(deccel),SetDWORDval(position),flag);
00282 }
00283 
00284 void RoboClaw::SpeedAccelDeccelPositionM2(uint32_t accel, int32_t speed, uint32_t deccel, int32_t position, uint8_t flag){
00285     write_n(19,address,M2SPEEDACCELDECCELPOS,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(deccel),SetDWORDval(position),flag);
00286 }
00287 
00288 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){
00289     write_n(35,address,MIXEDSPEEDACCELDECCELPOS,SetDWORDval(accel1),SetDWORDval(speed1),SetDWORDval(deccel1),SetDWORDval(position1),SetDWORDval(accel2),SetDWORDval(speed2),SetDWORDval(deccel2),SetDWORDval(position2),flag);
00290 }
00291