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