Jonathan Cacace / RoboClaw

xx

RoboClaw.cpp@3:5c6a9045c8f7, 2015-11-24 (annotated)

Committer:
sype
Date:
Tue Nov 24 21:56:10 2015 +0000
Revision:
3:5c6a9045c8f7
Parent:
2:a2c141d922b3
Child:
4:e64524a61cfe
Mise en forme

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