Marco De Silva / IONMcMotor

a

RoboClaw/RoboClaw.cpp@0:6b67f1bb9c76, 2021-09-28 (annotated)

Committer:
marcodesilva
Date:
Tue Sep 28 10:42:56 2021 +0000
Revision:
0:6b67f1bb9c76
d

Who changed what in which revision?

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