Jonathan Cacace / RoboClaw

xx

RoboClaw.cpp@13:6a4969d25627, 2021-09-14 (annotated)

Committer:
anfontanelli
Date:
Tue Sep 14 12:09:42 2021 +0000
Revision:
13:6a4969d25627
Parent:
11:04d8899b5d82 
A

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sype 0:af5cf35e1a25 1 #include "RoboClaw.h"
sype 0:af5cf35e1a25 2 #include <stdarg.h>
sype 0:af5cf35e1a25 3
sype 4:e64524a61cfe 4 #define MAXTRY 1
sype 2:a2c141d922b3 5 #define SetDWORDval(arg) (uint8_t)(arg>>24),(uint8_t)(arg>>16),(uint8_t)(arg>>8),(uint8_t)arg
sype 2:a2c141d922b3 6 #define SetWORDval(arg) (uint8_t)(arg>>8),(uint8_t)arg
sype 0:af5cf35e1a25 7
anfontanelli 13:6a4969d25627 8 RoboClaw::RoboClaw(uint8_t adr, int baudrate, PinName rx, PinName tx, double _read_timeout) : _roboclaw(rx, tx){
anfontanelli 11:04d8899b5d82 9 _roboclaw.set_baud(baudrate);//_roboclaw.baud(baudrate);
anfontanelli 11:04d8899b5d82 10
anfontanelli 11:04d8899b5d82 11 rx_in = 0;
sype 9:0fc5514faed9 12 address = adr;
anfontanelli 13:6a4969d25627 13 //readTime = 0.0;
anfontanelli 13:6a4969d25627 14 //readTimer.start();
anfontanelli 13:6a4969d25627 15 read_timeout = _read_timeout;
anfontanelli 11:04d8899b5d82 16
sype 0:af5cf35e1a25 17 }
sype 0:af5cf35e1a25 18
sype 3:5c6a9045c8f7 19 void RoboClaw::crc_clear(){
sype 0:af5cf35e1a25 20 crc = 0;
sype 0:af5cf35e1a25 21 }
sype 0:af5cf35e1a25 22
anfontanelli 11:04d8899b5d82 23
anfontanelli 11:04d8899b5d82 24 void RoboClaw::flushSerialBuffer(void) {
anfontanelli 11:04d8899b5d82 25 char char1[1];
anfontanelli 11:04d8899b5d82 26
anfontanelli 11:04d8899b5d82 27 while (_roboclaw.readable()) {
anfontanelli 11:04d8899b5d82 28 _roboclaw.read_timeout(char1,1,read_timeout);
anfontanelli 11:04d8899b5d82 29 }
anfontanelli 11:04d8899b5d82 30 return; }
anfontanelli 11:04d8899b5d82 31
sype 3:5c6a9045c8f7 32 void RoboClaw::crc_update (uint8_t data){
sype 0:af5cf35e1a25 33 int i;
sype 2:a2c141d922b3 34 crc = crc ^ ((uint16_t)data << 8);
sype 3:5c6a9045c8f7 35 for (i=0; i<8; i++) {
sype 0:af5cf35e1a25 36 if (crc & 0x8000)
sype 0:af5cf35e1a25 37 crc = (crc << 1) ^ 0x1021;
sype 0:af5cf35e1a25 38 else
sype 0:af5cf35e1a25 39 crc <<= 1;
sype 0:af5cf35e1a25 40 }
sype 0:af5cf35e1a25 41 }
sype 0:af5cf35e1a25 42
sype 3:5c6a9045c8f7 43 uint16_t RoboClaw::crc_get(){
sype 0:af5cf35e1a25 44 return crc;
sype 0:af5cf35e1a25 45 }
sype 0:af5cf35e1a25 46
sype 3:5c6a9045c8f7 47 uint16_t RoboClaw::crc16(uint8_t *packet, int nBytes){
sype 2:a2c141d922b3 48 uint16_t crc_;
sype 0:af5cf35e1a25 49 for (int byte = 0; byte < nBytes; byte++) {
sype 2:a2c141d922b3 50 crc_ = crc_ ^ ((uint16_t)packet[byte] << 8);
sype 2:a2c141d922b3 51 for (uint8_t bit = 0; bit < 8; bit++) {
sype 0:af5cf35e1a25 52 if (crc_ & 0x8000) {
sype 0:af5cf35e1a25 53 crc_ = (crc_ << 1) ^ 0x1021;
sype 0:af5cf35e1a25 54 } else {
sype 0:af5cf35e1a25 55 crc_ = crc_ << 1;
sype 0:af5cf35e1a25 56 }
sype 0:af5cf35e1a25 57 }
sype 0:af5cf35e1a25 58 }
sype 0:af5cf35e1a25 59 return crc_;
sype 0:af5cf35e1a25 60 }
sype 0:af5cf35e1a25 61
anfontanelli 11:04d8899b5d82 62 void RoboClaw::write_n(uint8_t cnt, ... ){
anfontanelli 11:04d8899b5d82 63 uint8_t count = 0;
anfontanelli 11:04d8899b5d82 64 char dat[1];
anfontanelli 11:04d8899b5d82 65 char r_dat[1];
anfontanelli 11:04d8899b5d82 66 r_dat[0] = ' ';
anfontanelli 11:04d8899b5d82 67 char crc_[2];
anfontanelli 11:04d8899b5d82 68 do {
anfontanelli 11:04d8899b5d82 69 _roboclaw.flush();
anfontanelli 11:04d8899b5d82 70
anfontanelli 11:04d8899b5d82 71 crc_clear();
anfontanelli 11:04d8899b5d82 72 va_list marker;
anfontanelli 11:04d8899b5d82 73 va_start( marker, cnt );
anfontanelli 11:04d8899b5d82 74 for(uint8_t index=0; index<cnt; index++) {
anfontanelli 11:04d8899b5d82 75 uint8_t data = va_arg(marker, unsigned int);
anfontanelli 11:04d8899b5d82 76 dat[0] = data;
anfontanelli 11:04d8899b5d82 77 crc_update(data);
anfontanelli 11:04d8899b5d82 78 _roboclaw.write(dat,1);
anfontanelli 11:04d8899b5d82 79 }
anfontanelli 11:04d8899b5d82 80 va_end( marker );
anfontanelli 11:04d8899b5d82 81 uint16_t crc = crc_get();
anfontanelli 11:04d8899b5d82 82 crc_[0]=crc>>8;
anfontanelli 11:04d8899b5d82 83 crc_[1]=crc;
anfontanelli 11:04d8899b5d82 84 _roboclaw.write(crc_,2);
anfontanelli 11:04d8899b5d82 85 _roboclaw.read_timeout(r_dat,1,read_timeout);
anfontanelli 11:04d8899b5d82 86
anfontanelli 11:04d8899b5d82 87 count ++;
anfontanelli 11:04d8899b5d82 88
anfontanelli 11:04d8899b5d82 89 }while((uint16_t)r_dat[0] != 0xFF&& count <=MAXTRY);
anfontanelli 11:04d8899b5d82 90
sype 0:af5cf35e1a25 91 }
sype 0:af5cf35e1a25 92
anfontanelli 11:04d8899b5d82 93 void RoboClaw::write_(uint8_t command, uint8_t data, bool reading, bool crcon){
anfontanelli 11:04d8899b5d82 94 // _roboclaw.putc(address);
anfontanelli 11:04d8899b5d82 95 // _roboclaw.putc(command);
anfontanelli 11:04d8899b5d82 96
anfontanelli 11:04d8899b5d82 97 if(reading == false) {
anfontanelli 11:04d8899b5d82 98 if(crcon == true) {
anfontanelli 11:04d8899b5d82 99 uint8_t packet[2] = {address, command};
anfontanelli 11:04d8899b5d82 100 uint16_t checksum = crc16(packet,2);
anfontanelli 11:04d8899b5d82 101 // _roboclaw.putc(checksum>>8);
anfontanelli 11:04d8899b5d82 102 // _roboclaw.putc(checksum);
anfontanelli 11:04d8899b5d82 103 } else {
anfontanelli 11:04d8899b5d82 104 uint8_t packet[3] = {address, command, data};
anfontanelli 11:04d8899b5d82 105 uint16_t checksum = crc16(packet,3);
anfontanelli 11:04d8899b5d82 106 // _roboclaw.putc(data);
anfontanelli 11:04d8899b5d82 107 // _roboclaw.putc(checksum>>8);
anfontanelli 11:04d8899b5d82 108 // _roboclaw.putc(checksum);
anfontanelli 11:04d8899b5d82 109 }
anfontanelli 11:04d8899b5d82 110 }
anfontanelli 11:04d8899b5d82 111 }
anfontanelli 11:04d8899b5d82 112
anfontanelli 11:04d8899b5d82 113 bool RoboClaw::read_n(uint8_t address, uint8_t cmd, int n_byte, int32_t &value){
anfontanelli 11:04d8899b5d82 114 uint16_t read_byte[n_byte];
anfontanelli 11:04d8899b5d82 115 uint16_t received_crc;
anfontanelli 11:04d8899b5d82 116 int dataRead=0;
anfontanelli 11:04d8899b5d82 117 char tx_buffer[2];
anfontanelli 11:04d8899b5d82 118
anfontanelli 11:04d8899b5d82 119 tx_buffer[0] = address;
anfontanelli 11:04d8899b5d82 120 tx_buffer[1] = cmd;
anfontanelli 11:04d8899b5d82 121
anfontanelli 11:04d8899b5d82 122 rx_in = 0;
anfontanelli 11:04d8899b5d82 123 crc_clear();
anfontanelli 11:04d8899b5d82 124
anfontanelli 11:04d8899b5d82 125 _roboclaw.flush();
anfontanelli 11:04d8899b5d82 126 _roboclaw.write(tx_buffer,2);
anfontanelli 11:04d8899b5d82 127
anfontanelli 11:04d8899b5d82 128 crc_update(address);
anfontanelli 11:04d8899b5d82 129 crc_update(cmd);
anfontanelli 11:04d8899b5d82 130
anfontanelli 11:04d8899b5d82 131 dataRead = _roboclaw.read_timeout(rx_buffer,7,read_timeout);
anfontanelli 11:04d8899b5d82 132 //printf("d: %d\r\n", dataRead);
anfontanelli 11:04d8899b5d82 133 //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]);
anfontanelli 11:04d8899b5d82 134 //printf("dopo del read\r\n");
anfontanelli 11:04d8899b5d82 135 for(int i = 0; i<=n_byte-3; i++){
anfontanelli 11:04d8899b5d82 136 crc_update((uint16_t)rx_buffer[i]);
anfontanelli 11:04d8899b5d82 137 }
anfontanelli 11:04d8899b5d82 138
anfontanelli 11:04d8899b5d82 139 received_crc = (uint16_t)rx_buffer[n_byte-2]<<8;
anfontanelli 11:04d8899b5d82 140 received_crc |= (uint16_t)rx_buffer[n_byte-1];
anfontanelli 11:04d8899b5d82 141
anfontanelli 11:04d8899b5d82 142 if(received_crc == crc_get()){
anfontanelli 11:04d8899b5d82 143 value = (uint16_t)rx_buffer[0]<<24;
anfontanelli 11:04d8899b5d82 144 value |= (uint16_t)rx_buffer[1]<<16;
anfontanelli 11:04d8899b5d82 145 value |= (uint16_t)rx_buffer[2]<<8;
anfontanelli 11:04d8899b5d82 146 value |= (uint16_t)rx_buffer[3];
anfontanelli 11:04d8899b5d82 147 return true;
anfontanelli 11:04d8899b5d82 148 }else{
anfontanelli 11:04d8899b5d82 149 //printf("crc: %x \r\n", crc_get());
anfontanelli 11:04d8899b5d82 150 //printf("received crc: %x \r\n", received_crc);
anfontanelli 13:6a4969d25627 151 //printf("err \r\n");
anfontanelli 11:04d8899b5d82 152
anfontanelli 11:04d8899b5d82 153
anfontanelli 11:04d8899b5d82 154 return false;
anfontanelli 11:04d8899b5d82 155 }
anfontanelli 11:04d8899b5d82 156 }
anfontanelli 11:04d8899b5d82 157
anfontanelli 11:04d8899b5d82 158
sype 9:0fc5514faed9 159 void RoboClaw::ForwardM1(int speed){
sype 9:0fc5514faed9 160 write_(M1FORWARD,speed,false,false);
sype 0:af5cf35e1a25 161 }
sype 0:af5cf35e1a25 162
sype 9:0fc5514faed9 163 void RoboClaw::BackwardM1(int speed){
sype 9:0fc5514faed9 164 write_(M1BACKWARD,speed,false,false);
sype 0:af5cf35e1a25 165 }
sype 0:af5cf35e1a25 166
sype 9:0fc5514faed9 167 void RoboClaw::ForwardM2(int speed){
sype 9:0fc5514faed9 168 write_(M2FORWARD,speed,false,false);
sype 0:af5cf35e1a25 169 }
sype 0:af5cf35e1a25 170
sype 9:0fc5514faed9 171 void RoboClaw::BackwardM2(int speed){
sype 9:0fc5514faed9 172 write_(M2BACKWARD,speed,false,false);
sype 0:af5cf35e1a25 173 }
sype 0:af5cf35e1a25 174
sype 9:0fc5514faed9 175 void RoboClaw::Forward(int speed){
sype 9:0fc5514faed9 176 write_(MIXEDFORWARD,speed,false,false);
sype 0:af5cf35e1a25 177 }
sype 0:af5cf35e1a25 178
sype 9:0fc5514faed9 179 void RoboClaw::Backward(int speed){
sype 9:0fc5514faed9 180 write_(MIXEDBACKWARD,speed,false,false);
sype 0:af5cf35e1a25 181 }
sype 0:af5cf35e1a25 182
sype 9:0fc5514faed9 183 void RoboClaw::ReadFirm(){
sype 9:0fc5514faed9 184 write_(GETVERSION,0x00,true,false);
sype 0:af5cf35e1a25 185 }
sype 0:af5cf35e1a25 186
anfontanelli 11:04d8899b5d82 187 bool RoboClaw::ReadEncM1(int32_t &encPulse){
anfontanelli 11:04d8899b5d82 188 int32_t value=0;
anfontanelli 11:04d8899b5d82 189 if (read_n(address, GETM1ENC, 7, value) == true){
anfontanelli 11:04d8899b5d82 190 encPulse = value;
anfontanelli 11:04d8899b5d82 191 return true;
anfontanelli 11:04d8899b5d82 192 }else{
anfontanelli 11:04d8899b5d82 193 return false;
anfontanelli 11:04d8899b5d82 194 }
anfontanelli 11:04d8899b5d82 195 }
sype 0:af5cf35e1a25 196
anfontanelli 11:04d8899b5d82 197 bool RoboClaw::ReadEncM2(int32_t &encPulse){
anfontanelli 11:04d8899b5d82 198 int32_t value=0;
anfontanelli 11:04d8899b5d82 199 if (read_n(address, GETM2ENC, 7, value) == true){
anfontanelli 11:04d8899b5d82 200 encPulse = value;
anfontanelli 11:04d8899b5d82 201 return true;
anfontanelli 11:04d8899b5d82 202 }else{
anfontanelli 11:04d8899b5d82 203 return false;
anfontanelli 11:04d8899b5d82 204 }
sype 0:af5cf35e1a25 205 }
sype 0:af5cf35e1a25 206
anfontanelli 11:04d8899b5d82 207 bool RoboClaw::ReadSpeedM1(int32_t &speed){
anfontanelli 11:04d8899b5d82 208 int32_t value=0;
anfontanelli 11:04d8899b5d82 209 if (read_n(address, GETM1SPEED, 7, value) == true){
anfontanelli 11:04d8899b5d82 210 speed = value;
anfontanelli 11:04d8899b5d82 211 return true;
anfontanelli 11:04d8899b5d82 212 }else{
anfontanelli 11:04d8899b5d82 213 return false;
anfontanelli 11:04d8899b5d82 214 }
sype 0:af5cf35e1a25 215 }
sype 0:af5cf35e1a25 216
anfontanelli 11:04d8899b5d82 217 bool RoboClaw::ReadSpeedM2(int32_t &speed){
anfontanelli 11:04d8899b5d82 218 int32_t value=0;
anfontanelli 11:04d8899b5d82 219 if (read_n(address, GETM2SPEED, 7, value) == true){
anfontanelli 11:04d8899b5d82 220 speed = value;
anfontanelli 11:04d8899b5d82 221 return true;
anfontanelli 11:04d8899b5d82 222 }else{
anfontanelli 11:04d8899b5d82 223 return false;
anfontanelli 11:04d8899b5d82 224 }
sype 0:af5cf35e1a25 225 }
sype 0:af5cf35e1a25 226
sype 0:af5cf35e1a25 227
anfontanelli 11:04d8899b5d82 228 bool RoboClaw::ReadCurrentM1M2(int32_t& currentM1, int32_t& currentM2){
anfontanelli 11:04d8899b5d82 229 int32_t value=0;
anfontanelli 11:04d8899b5d82 230 if (read_n(address, GETCURRENTS, 6, value) == true){
anfontanelli 11:04d8899b5d82 231 currentM1 = value>>16;
anfontanelli 11:04d8899b5d82 232 currentM2 = value&0xFFFF;
anfontanelli 11:04d8899b5d82 233 return true;
anfontanelli 11:04d8899b5d82 234 }else{
anfontanelli 11:04d8899b5d82 235 return false;
anfontanelli 11:04d8899b5d82 236 }
sype 3:5c6a9045c8f7 237
sype 0:af5cf35e1a25 238 }
sype 0:af5cf35e1a25 239
sype 9:0fc5514faed9 240 void RoboClaw::ResetEnc(){
sype 0:af5cf35e1a25 241 write_n(2,address,RESETENC);
sype 0:af5cf35e1a25 242 }
sype 0:af5cf35e1a25 243
sype 9:0fc5514faed9 244 void RoboClaw::SpeedM1(int32_t speed){
sype 0:af5cf35e1a25 245 write_n(6,address,M1SPEED,SetDWORDval(speed));
sype 0:af5cf35e1a25 246 }
sype 0:af5cf35e1a25 247
sype 9:0fc5514faed9 248 void RoboClaw::SpeedM2(int32_t speed){
sype 0:af5cf35e1a25 249 write_n(6,address,M2SPEED,SetDWORDval(speed));
sype 0:af5cf35e1a25 250 }
sype 0:af5cf35e1a25 251
sype 9:0fc5514faed9 252 void RoboClaw::SpeedAccelM1(int32_t accel, int32_t speed){
sype 0:af5cf35e1a25 253 write_n(10,address,M1SPEEDACCEL,SetDWORDval(accel),SetDWORDval(speed));
sype 0:af5cf35e1a25 254 }
sype 0:af5cf35e1a25 255
sype 9:0fc5514faed9 256 void RoboClaw::SpeedAccelM2(int32_t accel, int32_t speed){
sype 0:af5cf35e1a25 257 write_n(10,address,M2SPEEDACCEL,SetDWORDval(accel),SetDWORDval(speed));
sype 0:af5cf35e1a25 258 }
sype 0:af5cf35e1a25 259
sype 10:f82bc24e3bd4 260 void RoboClaw::SpeedAccelM1M2(int32_t accel, int32_t speed1, int32_t speed2){
sype 10:f82bc24e3bd4 261 write_n(14,address,MIXEDSPEEDACCEL,SetDWORDval(accel),SetDWORDval(speed1),SetDWORDval(speed2));
sype 5:ad00b3431ff5 262 }
sype 5:ad00b3431ff5 263
sype 9:0fc5514faed9 264 void RoboClaw::SpeedDistanceM1(int32_t speed, uint32_t distance, uint8_t buffer){
sype 0:af5cf35e1a25 265 write_n(11,address,M1SPEEDDIST,SetDWORDval(speed),SetDWORDval(distance),buffer);
sype 0:af5cf35e1a25 266 }
sype 0:af5cf35e1a25 267
sype 9:0fc5514faed9 268 void RoboClaw::SpeedDistanceM2(int32_t speed, uint32_t distance, uint8_t buffer){
sype 0:af5cf35e1a25 269 write_n(11,address,M2SPEEDDIST,SetDWORDval(speed),SetDWORDval(distance),buffer);
sype 0:af5cf35e1a25 270 }
sype 0:af5cf35e1a25 271
sype 9:0fc5514faed9 272 void RoboClaw::SpeedAccelDistanceM1(int32_t accel, int32_t speed, uint32_t distance, uint8_t buffer){
sype 0:af5cf35e1a25 273 write_n(15,address,M1SPEEDACCELDIST,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(distance),buffer);
sype 0:af5cf35e1a25 274 }
sype 0:af5cf35e1a25 275
sype 9:0fc5514faed9 276 void RoboClaw::SpeedAccelDistanceM2(int32_t accel, int32_t speed, uint32_t distance, uint8_t buffer){
sype 0:af5cf35e1a25 277 write_n(15,address,M2SPEEDACCELDIST,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(distance),buffer);
sype 0:af5cf35e1a25 278 }
sype 0:af5cf35e1a25 279
sype 9:0fc5514faed9 280 void RoboClaw::SpeedAccelDeccelPositionM1(uint32_t accel, int32_t speed, uint32_t deccel, int32_t position, uint8_t flag){
sype 1:f76058f9f548 281 write_n(19,address,M1SPEEDACCELDECCELPOS,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(deccel),SetDWORDval(position),flag);
sype 0:af5cf35e1a25 282 }
sype 0:af5cf35e1a25 283
sype 9:0fc5514faed9 284 void RoboClaw::SpeedAccelDeccelPositionM2(uint32_t accel, int32_t speed, uint32_t deccel, int32_t position, uint8_t flag){
sype 1:f76058f9f548 285 write_n(19,address,M2SPEEDACCELDECCELPOS,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(deccel),SetDWORDval(position),flag);
sype 1:f76058f9f548 286 }
sype 1:f76058f9f548 287
sype 9:0fc5514faed9 288 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){
sype 1:f76058f9f548 289 write_n(35,address,MIXEDSPEEDACCELDECCELPOS,SetDWORDval(accel1),SetDWORDval(speed1),SetDWORDval(deccel1),SetDWORDval(position1),SetDWORDval(accel2),SetDWORDval(speed2),SetDWORDval(deccel2),SetDWORDval(position2),flag);
sype 1:f76058f9f548 290 }
sype 1:f76058f9f548 291