yuki honma / UnderBody

下半身制御用ライブラリ Odometry...自己位置推定 Mecanum...メカナムホイール用 Bezier...ベジエ曲線 RoboClaw...MD用

Dependents:   TOUTEKI_all_mbed mbed_test_program

RoboClaw.cpp@3:9c3f2662974e, 2018-09-04 (annotated)

Committer:
yuki17100
Date:
Tue Sep 04 14:22:13 2018 +0000
Revision:
3:9c3f2662974e
Parent:
0:62707e16531a 
Mecanum????????

Who changed what in which revision?

UserRevisionLine numberNew 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