ARES / RoboClaw

oklm

Dependents:   RoboClaw TestMyPathFind Robot2016_2-0_STATIC Stressed ... more

RoboClaw.cpp@10:f82bc24e3bd4, 2016-05-04 (annotated)

Committer:
sype
Date:
Wed May 04 16:14:57 2016 +0000
Revision:
10:f82bc24e3bd4
Parent:
9:0fc5514faed9 
Je ne sais pas ce qui a chang? ici

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