xx
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
- Child:
- 11:04d8899b5d82
Je ne sais pas ce qui a chang? ici
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 | 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 |