a
RoboClaw/RoboClaw.cpp
- Committer:
- marcodesilva
- Date:
- 2021-09-28
- Revision:
- 0:6b67f1bb9c76
File content as of revision 0:6b67f1bb9c76:
#include "RoboClaw.h" #include <stdarg.h> #define MAXTRY 1 #define SetDWORDval(arg) (uint8_t)(arg>>24),(uint8_t)(arg>>16),(uint8_t)(arg>>8),(uint8_t)arg #define SetWORDval(arg) (uint8_t)(arg>>8),(uint8_t)arg RoboClaw::RoboClaw(uint8_t adr, int baudrate, PinName rx, PinName tx, double _read_timeout) : _roboclaw(rx, tx){ _roboclaw.set_baud(baudrate);//_roboclaw.baud(baudrate); rx_in = 0; address = adr; //readTime = 0.0; //readTimer.start(); read_timeout = _read_timeout; } void RoboClaw::crc_clear(){ crc = 0; } void RoboClaw::flushSerialBuffer(void) { char char1[1]; while (_roboclaw.readable()) { _roboclaw.read_timeout(char1,1,read_timeout); } return; } void RoboClaw::crc_update (uint8_t data){ int i; crc = crc ^ ((uint16_t)data << 8); for (i=0; i<8; i++) { if (crc & 0x8000) crc = (crc << 1) ^ 0x1021; else crc <<= 1; } } uint16_t RoboClaw::crc_get(){ return crc; } uint16_t RoboClaw::crc16(uint8_t *packet, int nBytes){ uint16_t crc_; for (int byte = 0; byte < nBytes; byte++) { crc_ = crc_ ^ ((uint16_t)packet[byte] << 8); for (uint8_t bit = 0; bit < 8; bit++) { if (crc_ & 0x8000) { crc_ = (crc_ << 1) ^ 0x1021; } else { crc_ = crc_ << 1; } } } return crc_; } void RoboClaw::write_n(uint8_t cnt, ... ){ uint8_t count = 0; char dat[1]; char r_dat[1]; r_dat[0] = ' '; char crc_[2]; do { _roboclaw.flush(); crc_clear(); va_list marker; va_start( marker, cnt ); for(uint8_t index=0; index<cnt; index++) { uint8_t data = va_arg(marker, unsigned int); dat[0] = data; crc_update(data); _roboclaw.write(dat,1); } va_end( marker ); uint16_t crc = crc_get(); crc_[0]=crc>>8; crc_[1]=crc; _roboclaw.write(crc_,2); _roboclaw.read_timeout(r_dat,1,read_timeout); count ++; }while((uint16_t)r_dat[0] != 0xFF&& count <=MAXTRY); } void RoboClaw::write_(uint8_t command, uint8_t data, bool reading, bool crcon){ // _roboclaw.putc(address); // _roboclaw.putc(command); if(reading == false) { if(crcon == true) { uint8_t packet[2] = {address, command}; uint16_t checksum = crc16(packet,2); // _roboclaw.putc(checksum>>8); // _roboclaw.putc(checksum); } else { uint8_t packet[3] = {address, command, data}; uint16_t checksum = crc16(packet,3); // _roboclaw.putc(data); // _roboclaw.putc(checksum>>8); // _roboclaw.putc(checksum); } } } bool RoboClaw::read_n(uint8_t address, uint8_t cmd, int n_byte, int32_t &value){ uint16_t read_byte[n_byte]; uint16_t received_crc; int dataRead=0; char tx_buffer[2]; tx_buffer[0] = address; tx_buffer[1] = cmd; rx_in = 0; crc_clear(); _roboclaw.flush(); _roboclaw.write(tx_buffer,2); crc_update(address); crc_update(cmd); dataRead = _roboclaw.read_timeout(rx_buffer,7,read_timeout); //printf("d: %d\r\n", dataRead); //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]); //printf("dopo del read\r\n"); for(int i = 0; i<=n_byte-3; i++){ crc_update((uint16_t)rx_buffer[i]); } received_crc = (uint16_t)rx_buffer[n_byte-2]<<8; received_crc |= (uint16_t)rx_buffer[n_byte-1]; if(received_crc == crc_get()){ value = (uint16_t)rx_buffer[0]<<24; value |= (uint16_t)rx_buffer[1]<<16; value |= (uint16_t)rx_buffer[2]<<8; value |= (uint16_t)rx_buffer[3]; return true; }else{ //printf("crc: %x \r\n", crc_get()); //printf("received crc: %x \r\n", received_crc); printf("err \r\n"); return false; } } void RoboClaw::ForwardM1(int speed){ write_(M1FORWARD,speed,false,false); } void RoboClaw::BackwardM1(int speed){ write_(M1BACKWARD,speed,false,false); } void RoboClaw::ForwardM2(int speed){ write_(M2FORWARD,speed,false,false); } void RoboClaw::BackwardM2(int speed){ write_(M2BACKWARD,speed,false,false); } void RoboClaw::Forward(int speed){ write_(MIXEDFORWARD,speed,false,false); } void RoboClaw::Backward(int speed){ write_(MIXEDBACKWARD,speed,false,false); } void RoboClaw::ReadFirm(){ write_(GETVERSION,0x00,true,false); } bool RoboClaw::ReadEncM1(int32_t &encPulse){ int32_t value=0; if (read_n(address, GETM1ENC, 7, value) == true){ encPulse = value; return true; }else{ return false; } } bool RoboClaw::ReadEncM2(int32_t &encPulse){ int32_t value=0; if (read_n(address, GETM2ENC, 7, value) == true){ encPulse = value; return true; }else{ return false; } } bool RoboClaw::ReadSpeedM1(int32_t &speed){ int32_t value=0; if (read_n(address, GETM1SPEED, 7, value) == true){ speed = value; return true; }else{ return false; } } bool RoboClaw::ReadSpeedM2(int32_t &speed){ int32_t value=0; if (read_n(address, GETM2SPEED, 7, value) == true){ speed = value; return true; }else{ return false; } } bool RoboClaw::ReadCurrentM1M2(int32_t& currentM1, int32_t& currentM2){ int32_t value=0; if (read_n(address, GETCURRENTS, 6, value) == true){ currentM1 = value>>16; currentM2 = value&0xFFFF; return true; }else{ return false; } } void RoboClaw::ResetEnc(){ write_n(2,address,RESETENC); } void RoboClaw::SpeedM1(int32_t speed){ write_n(6,address,M1SPEED,SetDWORDval(speed)); } void RoboClaw::SpeedM2(int32_t speed){ write_n(6,address,M2SPEED,SetDWORDval(speed)); } void RoboClaw::SpeedAccelM1(int32_t accel, int32_t speed){ write_n(10,address,M1SPEEDACCEL,SetDWORDval(accel),SetDWORDval(speed)); } void RoboClaw::SpeedAccelM2(int32_t accel, int32_t speed){ write_n(10,address,M2SPEEDACCEL,SetDWORDval(accel),SetDWORDval(speed)); } void RoboClaw::SpeedAccelM1M2(int32_t accel, int32_t speed1, int32_t speed2){ write_n(14,address,MIXEDSPEEDACCEL,SetDWORDval(accel),SetDWORDval(speed1),SetDWORDval(speed2)); } void RoboClaw::SpeedDistanceM1(int32_t speed, uint32_t distance, uint8_t buffer){ write_n(11,address,M1SPEEDDIST,SetDWORDval(speed),SetDWORDval(distance),buffer); } void RoboClaw::SpeedDistanceM2(int32_t speed, uint32_t distance, uint8_t buffer){ write_n(11,address,M2SPEEDDIST,SetDWORDval(speed),SetDWORDval(distance),buffer); } void RoboClaw::SpeedAccelDistanceM1(int32_t accel, int32_t speed, uint32_t distance, uint8_t buffer){ write_n(15,address,M1SPEEDACCELDIST,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(distance),buffer); } void RoboClaw::SpeedAccelDistanceM2(int32_t accel, int32_t speed, uint32_t distance, uint8_t buffer){ write_n(15,address,M2SPEEDACCELDIST,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(distance),buffer); } void RoboClaw::SpeedAccelDeccelPositionM1(uint32_t accel, int32_t speed, uint32_t deccel, int32_t position, uint8_t flag){ write_n(19,address,M1SPEEDACCELDECCELPOS,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(deccel),SetDWORDval(position),flag); } void RoboClaw::SpeedAccelDeccelPositionM2(uint32_t accel, int32_t speed, uint32_t deccel, int32_t position, uint8_t flag){ write_n(19,address,M2SPEEDACCELDECCELPOS,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(deccel),SetDWORDval(position),flag); } 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){ write_n(35,address,MIXEDSPEEDACCELDECCELPOS,SetDWORDval(accel1),SetDWORDval(speed1),SetDWORDval(deccel1),SetDWORDval(position1),SetDWORDval(accel2),SetDWORDval(speed2),SetDWORDval(deccel2),SetDWORDval(position2),flag); }