Jonathan Cacace / RoboClaw

xx

RoboClaw.cpp@0:af5cf35e1a25, 2015-11-16 (annotated)

Committer:
sype
Date:
Mon Nov 16 11:32:35 2015 +0000
Revision:
0:af5cf35e1a25
Child:
1:f76058f9f548
oklm

Who changed what in which revision?

UserRevisionLine numberNew 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 0:af5cf35e1a25 245 void RoboClaw::SpeedAccelDeccelPositionM1(unsigned char address, unsigned long accel, long speed, unsigned long deccel, unsigned long position, unsigned char flag){
sype 0:af5cf35e1a25 246 return write_n(19,address,M1SPEEDACCELDECCELPOS,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(deccel),SetDWORDval(position),flag);
sype 0:af5cf35e1a25 247 }
sype 0:af5cf35e1a25 248
sype 0:af5cf35e1a25 249 void RoboClaw::SpeedAccelDeccelPositionM2(unsigned char address, unsigned long accel, long speed, unsigned long deccel, unsigned long position, unsigned char flag){
sype 0:af5cf35e1a25 250 return write_n(19,address,M2SPEEDACCELDECCELPOS,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(deccel),SetDWORDval(position),flag);
sype 0:af5cf35e1a25 251 }