xx
RoboClaw.cpp@5:ad00b3431ff5, 2016-02-06 (annotated)
- Committer:
- sype
- Date:
- Sat Feb 06 10:11:15 2016 +0000
- Revision:
- 5:ad00b3431ff5
- Parent:
- 4:e64524a61cfe
- Child:
- 6:ece8a8cdf4b0
Command;
Who changed what in which revision?
User | Revision | Line number | New 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 | 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 | 4:e64524a61cfe | 32 | //uint8_t retry = MAXTRY; |
sype | 4:e64524a61cfe | 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 | 4:e64524a61cfe | 46 | //} while(_roboclaw.getc() != 0xFF); |
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 | 5:ad00b3431ff5 | 220 | void RoboClaw::SpeedAccelM1M2(uint8_t address, int32_t accel1, int32_t speed1, int32_t accel2, int32_t speed2){ |
sype | 5:ad00b3431ff5 | 221 | write_n(12,address,M2SPEEDACCEL,SetDWORDval(accel1),SetDWORDval(speed1),SetDWORDval(accel2),SetDWORDval(speed2)); |
sype | 5:ad00b3431ff5 | 222 | } |
sype | 5:ad00b3431ff5 | 223 | |
sype | 2:a2c141d922b3 | 224 | void RoboClaw::SpeedDistanceM1(uint8_t address, int32_t speed, uint32_t distance, uint8_t buffer){ |
sype | 0:af5cf35e1a25 | 225 | write_n(11,address,M1SPEEDDIST,SetDWORDval(speed),SetDWORDval(distance),buffer); |
sype | 0:af5cf35e1a25 | 226 | } |
sype | 0:af5cf35e1a25 | 227 | |
sype | 2:a2c141d922b3 | 228 | void RoboClaw::SpeedDistanceM2(uint8_t address, int32_t speed, uint32_t distance, uint8_t buffer){ |
sype | 0:af5cf35e1a25 | 229 | write_n(11,address,M2SPEEDDIST,SetDWORDval(speed),SetDWORDval(distance),buffer); |
sype | 0:af5cf35e1a25 | 230 | } |
sype | 0:af5cf35e1a25 | 231 | |
sype | 2:a2c141d922b3 | 232 | void RoboClaw::SpeedAccelDistanceM1(uint8_t address, int32_t accel, int32_t speed, uint32_t distance, uint8_t buffer){ |
sype | 0:af5cf35e1a25 | 233 | write_n(15,address,M1SPEEDACCELDIST,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(distance),buffer); |
sype | 0:af5cf35e1a25 | 234 | } |
sype | 0:af5cf35e1a25 | 235 | |
sype | 2:a2c141d922b3 | 236 | void RoboClaw::SpeedAccelDistanceM2(uint8_t address, int32_t accel, int32_t speed, uint32_t distance, uint8_t buffer){ |
sype | 0:af5cf35e1a25 | 237 | write_n(15,address,M2SPEEDACCELDIST,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(distance),buffer); |
sype | 0:af5cf35e1a25 | 238 | } |
sype | 0:af5cf35e1a25 | 239 | |
sype | 2:a2c141d922b3 | 240 | void RoboClaw::SpeedAccelDeccelPositionM1(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,M1SPEEDACCELDECCELPOS,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(deccel),SetDWORDval(position),flag); |
sype | 0:af5cf35e1a25 | 242 | } |
sype | 0:af5cf35e1a25 | 243 | |
sype | 2:a2c141d922b3 | 244 | void RoboClaw::SpeedAccelDeccelPositionM2(uint8_t address, uint32_t accel, int32_t speed, uint32_t deccel, int32_t position, uint8_t flag){ |
sype | 1:f76058f9f548 | 245 | write_n(19,address,M2SPEEDACCELDECCELPOS,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(deccel),SetDWORDval(position),flag); |
sype | 1:f76058f9f548 | 246 | } |
sype | 1:f76058f9f548 | 247 | |
sype | 2:a2c141d922b3 | 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){ |
sype | 1:f76058f9f548 | 249 | 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 | 250 | } |
sype | 1:f76058f9f548 | 251 |