Jonathan Cacace / RoboClaw

xx

RoboClaw.cpp@7:af0e3da221a9, 2016-04-25 (annotated)

Committer:
sype
Date:
Mon Apr 25 12:38:40 2016 +0000
Revision:
7:af0e3da221a9
Parent:
6:ece8a8cdf4b0
Child:
8:68669cccd769
impl?mentation de l'adresse dans le constructeur

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