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