2018_Project-R / RoboClaw_mine_

念のためパブリッシュ

RoboClaw.cpp@1:d36ea95c83f2, 2017-11-20 (annotated)

Committer:
kouika748
Date:
Mon Nov 20 06:57:47 2017 +0000
Revision:
1:d36ea95c83f2
Roboclaw???????????????????????????????????

Who changed what in which revision?

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