Marco De Silva / IONMcMotor_funzionante

ricreato il link mancante

RoboClaw/RoboClaw.cpp@4:31695331ce17, 2022-01-10 (annotated)

Committer:
marcodesilva
Date:
Mon Jan 10 20:38:41 2022 +0000
Revision:
4:31695331ce17
compila al 10/01/2022, da testare sul tool

Who changed what in which revision?

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