taka yamanouchi / MotorDrivers

first_library

Dependents:   2019_tourobo_upper minirobo_upper_reserve minirobo_under_reserve serial_RTX_NUCLEA

RoboClaw.cpp@1:9dd052490f0a, 2019-02-04 (annotated)

Committer:
sink
Date:
Mon Feb 04 04:46:37 2019 +0000
Revision:
1:9dd052490f0a
ok

Who changed what in which revision?

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