Index

auto-plow / 8b944c9

A wheelchair motor-propelled battery-powered ESP32-driven remote control snow plow.

Download | Refs | Commit Log | Files | README | LICENSE

Latest Commit

{#}TimeHashSubjectAuthor#(+)(-)GPG?
4418 Jan 2019 20:218c4a05dRoboClaw HeaderJoshua110500N

Blob @ auto-plow / include / arduino / RoboClaw.cpp

text/plain24800 bytesdownload raw
1#include "Arduino.h"
2#include "RoboClaw.h"
3
4#define MAXRETRY 2
5#define SetDWORDval(arg) (uint8_t)(((uint32_t)arg)>>24),(uint8_t)(((uint32_t)arg)>>16),(uint8_t)(((uint32_t)arg)>>8),(uint8_t)arg
6#define SetWORDval(arg) (uint8_t)(((uint16_t)arg)>>8),(uint8_t)arg
7
8//
9// Constructor
10//
11RoboClaw::RoboClaw(HardwareSerial *serial, uint32_t tout)
12{
13 timeout = tout;
14 hserial = serial;
15#ifdef __AVR__
16 sserial = 0;
17#endif
18}
19
20#ifdef __AVR__
21RoboClaw::RoboClaw(SoftwareSerial *serial, uint32_t tout)
22{
23 timeout = tout;
24 sserial = serial;
25 hserial = 0;
26}
27#endif
28
29//
30// Destructor
31//
32RoboClaw::~RoboClaw()
33{
34}
35
36void RoboClaw::begin(long speed)
37{
38 if(hserial){
39 hserial->begin(speed);
40 }
41#ifdef __AVR__
42 else{
43 sserial->begin(speed);
44 }
45#endif
46}
47
48bool RoboClaw::listen()
49{
50#ifdef __AVR__
51 if(sserial){
52 return sserial->listen();
53 }
54#endif
55 return false;
56}
57
58bool RoboClaw::isListening()
59{
60#ifdef __AVR__
61 if(sserial)
62 return sserial->isListening();
63#endif
64 return false;
65}
66
67bool RoboClaw::overflow()
68{
69#ifdef __AVR__
70 if(sserial)
71 return sserial->overflow();
72#endif
73 return false;
74}
75
76int RoboClaw::peek()
77{
78 if(hserial)
79 return hserial->peek();
80#ifdef __AVR__
81 else
82 return sserial->peek();
83#endif
84}
85
86size_t RoboClaw::write(uint8_t byte)
87{
88 if(hserial)
89 return hserial->write(byte);
90#ifdef __AVR__
91 else
92 return sserial->write(byte);
93#endif
94}
95
96int RoboClaw::read()
97{
98 if(hserial)
99 return hserial->read();
100#ifdef __AVR__
101 else
102 return sserial->read();
103#endif
104}
105
106int RoboClaw::available()
107{
108 if(hserial)
109 return hserial->available();
110#ifdef __AVR__
111 else
112 return sserial->available();
113#endif
114}
115
116void RoboClaw::flush()
117{
118 if(hserial)
119 hserial->flush();
120}
121
122int RoboClaw::read(uint32_t timeout)
123{
124 if(hserial){
125 uint32_t start = micros();
126 // Empty buffer?
127 while(!hserial->available()){
128 if((micros()-start)>=timeout)
129 return -1;
130 }
131 return hserial->read();
132 }
133#ifdef __AVR__
134 else{
135 if(sserial->isListening()){
136 uint32_t start = micros();
137 // Empty buffer?
138 while(!sserial->available()){
139 if((micros()-start)>=timeout)
140 return -1;
141 }
142 return sserial->read();
143 }
144 }
145#endif
146}
147
148void RoboClaw::clear()
149{
150 if(hserial){
151 while(hserial->available())
152 hserial->read();
153 }
154#ifdef __AVR__
155 else{
156 while(sserial->available())
157 sserial->read();
158 }
159#endif
160}
161
162void RoboClaw::crc_clear()
163{
164 crc = 0;
165}
166
167void RoboClaw::crc_update (uint8_t data)
168{
169 int i;
170 crc = crc ^ ((uint16_t)data << 8);
171 for (i=0; i<8; i++)
172 {
173 if (crc & 0x8000)
174 crc = (crc << 1) ^ 0x1021;
175 else
176 crc <<= 1;
177 }
178}
179
180uint16_t RoboClaw::crc_get()
181{
182 return crc;
183}
184
185bool RoboClaw::write_n(uint8_t cnt, ... )
186{
187 uint8_t trys=MAXRETRY;
188 do{
189 crc_clear();
190 //send data with crc
191 va_list marker;
192 va_start( marker, cnt ); /* Initialize variable arguments. */
193 for(uint8_t index=0;index<cnt;index++){
194 uint8_t data = va_arg(marker, int);
195 crc_update(data);
196 write(data);
197 }
198 va_end( marker ); /* Reset variable arguments. */
199 uint16_t crc = crc_get();
200 write(crc>>8);
201 write(crc);
202 if(read(timeout)==0xFF)
203 return true;
204 }while(trys--);
205 return false;
206}
207
208bool RoboClaw::read_n(uint8_t cnt,uint8_t address,uint8_t cmd,...)
209{
210 uint32_t value=0;
211 uint8_t trys=MAXRETRY;
212 int16_t data;
213 do{
214 flush();
215
216 data=0;
217 crc_clear();
218 write(address);
219 crc_update(address);
220 write(cmd);
221 crc_update(cmd);
222
223 //send data with crc
224 va_list marker;
225 va_start( marker, cmd ); /* Initialize variable arguments. */
226 for(uint8_t index=0;index<cnt;index++){
227 uint32_t *ptr = va_arg(marker, uint32_t *);
228
229 if(data!=-1){
230 data = read(timeout);
231 crc_update(data);
232 value=(uint32_t)data<<24;
233 }
234 else{
235 break;
236 }
237
238 if(data!=-1){
239 data = read(timeout);
240 crc_update(data);
241 value|=(uint32_t)data<<16;
242 }
243 else{
244 break;
245 }
246
247 if(data!=-1){
248 data = read(timeout);
249 crc_update(data);
250 value|=(uint32_t)data<<8;
251 }
252 else{
253 break;
254 }
255
256 if(data!=-1){
257 data = read(timeout);
258 crc_update(data);
259 value|=(uint32_t)data;
260 }
261 else{
262 break;
263 }
264
265 *ptr = value;
266 }
267 va_end( marker ); /* Reset variable arguments. */
268
269 if(data!=-1){
270 uint16_t ccrc;
271 data = read(timeout);
272 if(data!=-1){
273 ccrc = data << 8;
274 data = read(timeout);
275 if(data!=-1){
276 ccrc |= data;
277 return crc_get()==ccrc;
278 }
279 }
280 }
281 }while(trys--);
282
283 return false;
284}
285
286uint8_t RoboClaw::Read1(uint8_t address,uint8_t cmd,bool *valid){
287 uint8_t crc;
288
289 if(valid)
290 *valid = false;
291
292 uint8_t value=0;
293 uint8_t trys=MAXRETRY;
294 int16_t data;
295 do{
296 flush();
297
298 crc_clear();
299 write(address);
300 crc_update(address);
301 write(cmd);
302 crc_update(cmd);
303
304 data = read(timeout);
305 crc_update(data);
306 value=data;
307
308 if(data!=-1){
309 uint16_t ccrc;
310 data = read(timeout);
311 if(data!=-1){
312 ccrc = data << 8;
313 data = read(timeout);
314 if(data!=-1){
315 ccrc |= data;
316 if(crc_get()==ccrc){
317 *valid = true;
318 return value;
319 }
320 }
321 }
322 }
323 }while(trys--);
324
325 return false;
326}
327
328uint16_t RoboClaw::Read2(uint8_t address,uint8_t cmd,bool *valid){
329 uint8_t crc;
330
331 if(valid)
332 *valid = false;
333
334 uint16_t value=0;
335 uint8_t trys=MAXRETRY;
336 int16_t data;
337 do{
338 flush();
339
340 crc_clear();
341 write(address);
342 crc_update(address);
343 write(cmd);
344 crc_update(cmd);
345
346 data = read(timeout);
347 crc_update(data);
348 value=(uint16_t)data<<8;
349
350 if(data!=-1){
351 data = read(timeout);
352 crc_update(data);
353 value|=(uint16_t)data;
354 }
355
356 if(data!=-1){
357 uint16_t ccrc;
358 data = read(timeout);
359 if(data!=-1){
360 ccrc = data << 8;
361 data = read(timeout);
362 if(data!=-1){
363 ccrc |= data;
364 if(crc_get()==ccrc){
365 *valid = true;
366 return value;
367 }
368 }
369 }
370 }
371 }while(trys--);
372
373 return false;
374}
375
376uint32_t RoboClaw::Read4(uint8_t address, uint8_t cmd, bool *valid){
377 uint8_t crc;
378
379 if(valid)
380 *valid = false;
381
382 uint32_t value=0;
383 uint8_t trys=MAXRETRY;
384 int16_t data;
385 do{
386 flush();
387
388 crc_clear();
389 write(address);
390 crc_update(address);
391 write(cmd);
392 crc_update(cmd);
393
394 data = read(timeout);
395 crc_update(data);
396 value=(uint32_t)data<<24;
397
398 if(data!=-1){
399 data = read(timeout);
400 crc_update(data);
401 value|=(uint32_t)data<<16;
402 }
403
404 if(data!=-1){
405 data = read(timeout);
406 crc_update(data);
407 value|=(uint32_t)data<<8;
408 }
409
410 if(data!=-1){
411 data = read(timeout);
412 crc_update(data);
413 value|=(uint32_t)data;
414 }
415
416 if(data!=-1){
417 uint16_t ccrc;
418 data = read(timeout);
419 if(data!=-1){
420 ccrc = data << 8;
421 data = read(timeout);
422 if(data!=-1){
423 ccrc |= data;
424 if(crc_get()==ccrc){
425 *valid = true;
426 return value;
427 }
428 }
429 }
430 }
431 }while(trys--);
432
433 return false;
434}
435
436uint32_t RoboClaw::Read4_1(uint8_t address, uint8_t cmd, uint8_t *status, bool *valid){
437 uint8_t crc;
438
439 if(valid)
440 *valid = false;
441
442 uint32_t value=0;
443 uint8_t trys=MAXRETRY;
444 int16_t data;
445 do{
446 flush();
447
448 crc_clear();
449 write(address);
450 crc_update(address);
451 write(cmd);
452 crc_update(cmd);
453
454 data = read(timeout);
455 crc_update(data);
456 value=(uint32_t)data<<24;
457
458 if(data!=-1){
459 data = read(timeout);
460 crc_update(data);
461 value|=(uint32_t)data<<16;
462 }
463
464 if(data!=-1){
465 data = read(timeout);
466 crc_update(data);
467 value|=(uint32_t)data<<8;
468 }
469
470 if(data!=-1){
471 data = read(timeout);
472 crc_update(data);
473 value|=(uint32_t)data;
474 }
475
476 if(data!=-1){
477 data = read(timeout);
478 crc_update(data);
479 if(status)
480 *status = data;
481 }
482
483 if(data!=-1){
484 uint16_t ccrc;
485 data = read(timeout);
486 if(data!=-1){
487 ccrc = data << 8;
488 data = read(timeout);
489 if(data!=-1){
490 ccrc |= data;
491 if(crc_get()==ccrc){
492 *valid = true;
493 return value;
494 }
495 }
496 }
497 }
498 }while(trys--);
499
500 return false;
501}
502
503bool RoboClaw::ForwardM1(uint8_t address, uint8_t speed){
504 return write_n(3,address,M1FORWARD,speed);
505}
506
507bool RoboClaw::BackwardM1(uint8_t address, uint8_t speed){
508 return write_n(3,address,M1BACKWARD,speed);
509}
510
511bool RoboClaw::SetMinVoltageMainBattery(uint8_t address, uint8_t voltage){
512 return write_n(3,address,SETMINMB,voltage);
513}
514
515bool RoboClaw::SetMaxVoltageMainBattery(uint8_t address, uint8_t voltage){
516 return write_n(3,address,SETMAXMB,voltage);
517}
518
519bool RoboClaw::ForwardM2(uint8_t address, uint8_t speed){
520 return write_n(3,address,M2FORWARD,speed);
521}
522
523bool RoboClaw::BackwardM2(uint8_t address, uint8_t speed){
524 return write_n(3,address,M2BACKWARD,speed);
525}
526
527bool RoboClaw::ForwardBackwardM1(uint8_t address, uint8_t speed){
528 return write_n(3,address,M17BIT,speed);
529}
530
531bool RoboClaw::ForwardBackwardM2(uint8_t address, uint8_t speed){
532 return write_n(3,address,M27BIT,speed);
533}
534
535bool RoboClaw::ForwardMixed(uint8_t address, uint8_t speed){
536 return write_n(3,address,MIXEDFORWARD,speed);
537}
538
539bool RoboClaw::BackwardMixed(uint8_t address, uint8_t speed){
540 return write_n(3,address,MIXEDBACKWARD,speed);
541}
542
543bool RoboClaw::TurnRightMixed(uint8_t address, uint8_t speed){
544 return write_n(3,address,MIXEDRIGHT,speed);
545}
546
547bool RoboClaw::TurnLeftMixed(uint8_t address, uint8_t speed){
548 return write_n(3,address,MIXEDLEFT,speed);
549}
550
551bool RoboClaw::ForwardBackwardMixed(uint8_t address, uint8_t speed){
552 return write_n(3,address,MIXEDFB,speed);
553}
554
555bool RoboClaw::LeftRightMixed(uint8_t address, uint8_t speed){
556 return write_n(3,address,MIXEDLR,speed);
557}
558
559uint32_t RoboClaw::ReadEncM1(uint8_t address, uint8_t *status,bool *valid){
560 return Read4_1(address,GETM1ENC,status,valid);
561}
562
563uint32_t RoboClaw::ReadEncM2(uint8_t address, uint8_t *status,bool *valid){
564 return Read4_1(address,GETM2ENC,status,valid);
565}
566
567uint32_t RoboClaw::ReadSpeedM1(uint8_t address, uint8_t *status,bool *valid){
568 return Read4_1(address,GETM1SPEED,status,valid);
569}
570
571uint32_t RoboClaw::ReadSpeedM2(uint8_t address, uint8_t *status,bool *valid){
572 return Read4_1(address,GETM2SPEED,status,valid);
573}
574
575bool RoboClaw::ResetEncoders(uint8_t address){
576 return write_n(2,address,RESETENC);
577}
578
579bool RoboClaw::ReadVersion(uint8_t address,char *version){
580 uint8_t data;
581 uint8_t trys=MAXRETRY;
582 do{
583 flush();
584
585 data = 0;
586
587 crc_clear();
588 write(address);
589 crc_update(address);
590 write(GETVERSION);
591 crc_update(GETVERSION);
592
593 uint8_t i;
594 for(i=0;i<48;i++){
595 if(data!=-1){
596 data=read(timeout);
597 version[i] = data;
598 crc_update(version[i]);
599 if(version[i]==0){
600 uint16_t ccrc;
601 data = read(timeout);
602 if(data!=-1){
603 ccrc = data << 8;
604 data = read(timeout);
605 if(data!=-1){
606 ccrc |= data;
607 return crc_get()==ccrc;
608 }
609 }
610 break;
611 }
612 }
613 else{
614 break;
615 }
616 }
617 }while(trys--);
618
619 return false;
620}
621
622bool RoboClaw::SetEncM1(uint8_t address, int32_t val){
623 return write_n(6,address,SETM1ENCCOUNT,SetDWORDval(val));
624}
625
626bool RoboClaw::SetEncM2(uint8_t address, int32_t val){
627 return write_n(6,address,SETM2ENCCOUNT,SetDWORDval(val));
628}
629
630uint16_t RoboClaw::ReadMainBatteryVoltage(uint8_t address,bool *valid){
631 return Read2(address,GETMBATT,valid);
632}
633
634uint16_t RoboClaw::ReadLogicBatteryVoltage(uint8_t address,bool *valid){
635 return Read2(address,GETLBATT,valid);
636}
637
638bool RoboClaw::SetMinVoltageLogicBattery(uint8_t address, uint8_t voltage){
639 return write_n(3,address,SETMINLB,voltage);
640}
641
642bool RoboClaw::SetMaxVoltageLogicBattery(uint8_t address, uint8_t voltage){
643 return write_n(3,address,SETMAXLB,voltage);
644}
645
646bool RoboClaw::SetM1VelocityPID(uint8_t address, float kp_fp, float ki_fp, float kd_fp, uint32_t qpps){
647 uint32_t kp = kp_fp*65536;
648 uint32_t ki = ki_fp*65536;
649 uint32_t kd = kd_fp*65536;
650 return write_n(18,address,SETM1PID,SetDWORDval(kd),SetDWORDval(kp),SetDWORDval(ki),SetDWORDval(qpps));
651}
652
653bool RoboClaw::SetM2VelocityPID(uint8_t address, float kp_fp, float ki_fp, float kd_fp, uint32_t qpps){
654 uint32_t kp = kp_fp*65536;
655 uint32_t ki = ki_fp*65536;
656 uint32_t kd = kd_fp*65536;
657 return write_n(18,address,SETM2PID,SetDWORDval(kd),SetDWORDval(kp),SetDWORDval(ki),SetDWORDval(qpps));
658}
659
660uint32_t RoboClaw::ReadISpeedM1(uint8_t address,uint8_t *status,bool *valid){
661 return Read4_1(address,GETM1ISPEED,status,valid);
662}
663
664uint32_t RoboClaw::ReadISpeedM2(uint8_t address,uint8_t *status,bool *valid){
665 return Read4_1(address,GETM2ISPEED,status,valid);
666}
667
668bool RoboClaw::DutyM1(uint8_t address, uint16_t duty){
669 return write_n(4,address,M1DUTY,SetWORDval(duty));
670}
671
672bool RoboClaw::DutyM2(uint8_t address, uint16_t duty){
673 return write_n(4,address,M2DUTY,SetWORDval(duty));
674}
675
676bool RoboClaw::DutyM1M2(uint8_t address, uint16_t duty1, uint16_t duty2){
677 return write_n(6,address,MIXEDDUTY,SetWORDval(duty1),SetWORDval(duty2));
678}
679
680bool RoboClaw::SpeedM1(uint8_t address, uint32_t speed){
681 return write_n(6,address,M1SPEED,SetDWORDval(speed));
682}
683
684bool RoboClaw::SpeedM2(uint8_t address, uint32_t speed){
685 return write_n(6,address,M2SPEED,SetDWORDval(speed));
686}
687
688bool RoboClaw::SpeedM1M2(uint8_t address, uint32_t speed1, uint32_t speed2){
689 return write_n(10,address,MIXEDSPEED,SetDWORDval(speed1),SetDWORDval(speed2));
690}
691
692bool RoboClaw::SpeedAccelM1(uint8_t address, uint32_t accel, uint32_t speed){
693 return write_n(10,address,M1SPEEDACCEL,SetDWORDval(accel),SetDWORDval(speed));
694}
695
696bool RoboClaw::SpeedAccelM2(uint8_t address, uint32_t accel, uint32_t speed){
697 return write_n(10,address,M2SPEEDACCEL,SetDWORDval(accel),SetDWORDval(speed));
698}
699bool RoboClaw::SpeedAccelM1M2(uint8_t address, uint32_t accel, uint32_t speed1, uint32_t speed2){
700 return write_n(14,address,MIXEDSPEEDACCEL,SetDWORDval(accel),SetDWORDval(speed1),SetDWORDval(speed2));
701}
702
703bool RoboClaw::SpeedDistanceM1(uint8_t address, uint32_t speed, uint32_t distance, uint8_t flag){
704 return write_n(11,address,M1SPEEDDIST,SetDWORDval(speed),SetDWORDval(distance),flag);
705}
706
707bool RoboClaw::SpeedDistanceM2(uint8_t address, uint32_t speed, uint32_t distance, uint8_t flag){
708 return write_n(11,address,M2SPEEDDIST,SetDWORDval(speed),SetDWORDval(distance),flag);
709}
710
711bool RoboClaw::SpeedDistanceM1M2(uint8_t address, uint32_t speed1, uint32_t distance1, uint32_t speed2, uint32_t distance2, uint8_t flag){
712 return write_n(19,address,MIXEDSPEEDDIST,SetDWORDval(speed1),SetDWORDval(distance1),SetDWORDval(speed2),SetDWORDval(distance2),flag);
713}
714
715bool RoboClaw::SpeedAccelDistanceM1(uint8_t address, uint32_t accel, uint32_t speed, uint32_t distance, uint8_t flag){
716 return write_n(15,address,M1SPEEDACCELDIST,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(distance),flag);
717}
718
719bool RoboClaw::SpeedAccelDistanceM2(uint8_t address, uint32_t accel, uint32_t speed, uint32_t distance, uint8_t flag){
720 return write_n(15,address,M2SPEEDACCELDIST,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(distance),flag);
721}
722
723bool RoboClaw::SpeedAccelDistanceM1M2(uint8_t address, uint32_t accel, uint32_t speed1, uint32_t distance1, uint32_t speed2, uint32_t distance2, uint8_t flag){
724 return write_n(23,address,MIXEDSPEEDACCELDIST,SetDWORDval(accel),SetDWORDval(speed1),SetDWORDval(distance1),SetDWORDval(speed2),SetDWORDval(distance2),flag);
725}
726
727bool RoboClaw::ReadBuffers(uint8_t address, uint8_t &depth1, uint8_t &depth2){
728 bool valid;
729 uint16_t value = Read2(address,GETBUFFERS,&valid);
730 if(valid){
731 depth1 = value>>8;
732 depth2 = value;
733 }
734 return valid;
735}
736
737bool RoboClaw::ReadPWMs(uint8_t address, int16_t &pwm1, int16_t &pwm2){
738 bool valid;
739 uint32_t value = Read4(address,GETPWMS,&valid);
740 if(valid){
741 pwm1 = value>>16;
742 pwm2 = value&0xFFFF;
743 }
744 return valid;
745}
746
747bool RoboClaw::ReadCurrents(uint8_t address, int16_t &current1, int16_t &current2){
748 bool valid;
749 uint32_t value = Read4(address,GETCURRENTS,&valid);
750 if(valid){
751 current1 = value>>16;
752 current2 = value&0xFFFF;
753 }
754 return valid;
755}
756
757bool RoboClaw::SpeedAccelM1M2_2(uint8_t address, uint32_t accel1, uint32_t speed1, uint32_t accel2, uint32_t speed2){
758 return write_n(18,address,MIXEDSPEED2ACCEL,SetDWORDval(accel1),SetDWORDval(speed1),SetDWORDval(accel2),SetDWORDval(speed2));
759}
760
761bool RoboClaw::SpeedAccelDistanceM1M2_2(uint8_t address, uint32_t accel1, uint32_t speed1, uint32_t distance1, uint32_t accel2, uint32_t speed2, uint32_t distance2, uint8_t flag){
762 return write_n(27,address,MIXEDSPEED2ACCELDIST,SetDWORDval(accel1),SetDWORDval(speed1),SetDWORDval(distance1),SetDWORDval(accel2),SetDWORDval(speed2),SetDWORDval(distance2),flag);
763}
764
765bool RoboClaw::DutyAccelM1(uint8_t address, uint16_t duty, uint32_t accel){
766 return write_n(8,address,M1DUTYACCEL,SetWORDval(duty),SetDWORDval(accel));
767}
768
769bool RoboClaw::DutyAccelM2(uint8_t address, uint16_t duty, uint32_t accel){
770 return write_n(8,address,M2DUTYACCEL,SetWORDval(duty),SetDWORDval(accel));
771}
772
773bool RoboClaw::DutyAccelM1M2(uint8_t address, uint16_t duty1, uint32_t accel1, uint16_t duty2, uint32_t accel2){
774 return write_n(14,address,MIXEDDUTYACCEL,SetWORDval(duty1),SetDWORDval(accel1),SetWORDval(duty2),SetDWORDval(accel2));
775}
776
777bool RoboClaw::ReadM1VelocityPID(uint8_t address,float &Kp_fp,float &Ki_fp,float &Kd_fp,uint32_t &qpps){
778 uint32_t Kp,Ki,Kd;
779 bool valid = read_n(4,address,READM1PID,&Kp,&Ki,&Kd,&qpps);
780 Kp_fp = ((float)Kp)/65536;
781 Ki_fp = ((float)Ki)/65536;
782 Kd_fp = ((float)Kd)/65536;
783 return valid;
784}
785
786bool RoboClaw::ReadM2VelocityPID(uint8_t address,float &Kp_fp,float &Ki_fp,float &Kd_fp,uint32_t &qpps){
787 uint32_t Kp,Ki,Kd;
788 bool valid = read_n(4,address,READM2PID,&Kp,&Ki,&Kd,&qpps);
789 Kp_fp = ((float)Kp)/65536;
790 Ki_fp = ((float)Ki)/65536;
791 Kd_fp = ((float)Kd)/65536;
792 return valid;
793}
794
795bool RoboClaw::SetMainVoltages(uint8_t address,uint16_t min,uint16_t max){
796 return write_n(6,address,SETMAINVOLTAGES,SetWORDval(min),SetWORDval(max));
797}
798
799bool RoboClaw::SetLogicVoltages(uint8_t address,uint16_t min,uint16_t max){
800 return write_n(6,address,SETLOGICVOLTAGES,SetWORDval(min),SetWORDval(max));
801}
802
803bool RoboClaw::ReadMinMaxMainVoltages(uint8_t address,uint16_t &min,uint16_t &max){
804 bool valid;
805 uint32_t value = Read4(address,GETMINMAXMAINVOLTAGES,&valid);
806 if(valid){
807 min = value>>16;
808 max = value&0xFFFF;
809 }
810 return valid;
811}
812
813bool RoboClaw::ReadMinMaxLogicVoltages(uint8_t address,uint16_t &min,uint16_t &max){
814 bool valid;
815 uint32_t value = Read4(address,GETMINMAXLOGICVOLTAGES,&valid);
816 if(valid){
817 min = value>>16;
818 max = value&0xFFFF;
819 }
820 return valid;
821}
822
823bool RoboClaw::SetM1PositionPID(uint8_t address,float kp_fp,float ki_fp,float kd_fp,uint32_t kiMax,uint32_t deadzone,uint32_t min,uint32_t max){
824 uint32_t kp=kp_fp*1024;
825 uint32_t ki=ki_fp*1024;
826 uint32_t kd=kd_fp*1024;
827 return write_n(30,address,SETM1POSPID,SetDWORDval(kd),SetDWORDval(kp),SetDWORDval(ki),SetDWORDval(kiMax),SetDWORDval(deadzone),SetDWORDval(min),SetDWORDval(max));
828}
829
830bool RoboClaw::SetM2PositionPID(uint8_t address,float kp_fp,float ki_fp,float kd_fp,uint32_t kiMax,uint32_t deadzone,uint32_t min,uint32_t max){
831 uint32_t kp=kp_fp*1024;
832 uint32_t ki=ki_fp*1024;
833 uint32_t kd=kd_fp*1024;
834 return write_n(30,address,SETM2POSPID,SetDWORDval(kd),SetDWORDval(kp),SetDWORDval(ki),SetDWORDval(kiMax),SetDWORDval(deadzone),SetDWORDval(min),SetDWORDval(max));
835}
836
837bool RoboClaw::ReadM1PositionPID(uint8_t address,float &Kp_fp,float &Ki_fp,float &Kd_fp,uint32_t &KiMax,uint32_t &DeadZone,uint32_t &Min,uint32_t &Max){
838 uint32_t Kp,Ki,Kd;
839 bool valid = read_n(7,address,READM1POSPID,&Kp,&Ki,&Kd,&KiMax,&DeadZone,&Min,&Max);
840 Kp_fp = ((float)Kp)/1024;
841 Ki_fp = ((float)Ki)/1024;
842 Kd_fp = ((float)Kd)/1024;
843 return valid;
844}
845
846bool RoboClaw::ReadM2PositionPID(uint8_t address,float &Kp_fp,float &Ki_fp,float &Kd_fp,uint32_t &KiMax,uint32_t &DeadZone,uint32_t &Min,uint32_t &Max){
847 uint32_t Kp,Ki,Kd;
848 bool valid = read_n(7,address,READM2POSPID,&Kp,&Ki,&Kd,&KiMax,&DeadZone,&Min,&Max);
849 Kp_fp = ((float)Kp)/1024;
850 Ki_fp = ((float)Ki)/1024;
851 Kd_fp = ((float)Kd)/1024;
852 return valid;
853}
854
855bool RoboClaw::SpeedAccelDeccelPositionM1(uint8_t address,uint32_t accel,uint32_t speed,uint32_t deccel,uint32_t position,uint8_t flag){
856 return write_n(19,address,M1SPEEDACCELDECCELPOS,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(deccel),SetDWORDval(position),flag);
857}
858
859bool RoboClaw::SpeedAccelDeccelPositionM2(uint8_t address,uint32_t accel,uint32_t speed,uint32_t deccel,uint32_t position,uint8_t flag){
860 return write_n(19,address,M2SPEEDACCELDECCELPOS,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(deccel),SetDWORDval(position),flag);
861}
862
863bool RoboClaw::SpeedAccelDeccelPositionM1M2(uint8_t address,uint32_t accel1,uint32_t speed1,uint32_t deccel1,uint32_t position1,uint32_t accel2,uint32_t speed2,uint32_t deccel2,uint32_t position2,uint8_t flag){
864 return write_n(35,address,MIXEDSPEEDACCELDECCELPOS,SetDWORDval(accel1),SetDWORDval(speed1),SetDWORDval(deccel1),SetDWORDval(position1),SetDWORDval(accel2),SetDWORDval(speed2),SetDWORDval(deccel2),SetDWORDval(position2),flag);
865}
866
867bool RoboClaw::SetM1DefaultAccel(uint8_t address, uint32_t accel){
868 return write_n(6,address,SETM1DEFAULTACCEL,SetDWORDval(accel));
869}
870
871bool RoboClaw::SetM2DefaultAccel(uint8_t address, uint32_t accel){
872 return write_n(6,address,SETM2DEFAULTACCEL,SetDWORDval(accel));
873}
874
875bool RoboClaw::SetPinFunctions(uint8_t address, uint8_t S3mode, uint8_t S4mode, uint8_t S5mode){
876 return write_n(5,address,SETPINFUNCTIONS,S3mode,S4mode,S5mode);
877}
878
879bool RoboClaw::GetPinFunctions(uint8_t address, uint8_t &S3mode, uint8_t &S4mode, uint8_t &S5mode){
880 uint8_t crc;
881 bool valid = false;
882 uint8_t val1,val2,val3;
883 uint8_t trys=MAXRETRY;
884 int16_t data;
885 do{
886 flush();
887
888 crc_clear();
889 write(address);
890 crc_update(address);
891 write(GETPINFUNCTIONS);
892 crc_update(GETPINFUNCTIONS);
893
894 data = read(timeout);
895 crc_update(data);
896 val1=data;
897
898 if(data!=-1){
899 data = read(timeout);
900 crc_update(data);
901 val2=data;
902 }
903
904 if(data!=-1){
905 data = read(timeout);
906 crc_update(data);
907 val3=data;
908 }
909
910 if(data!=-1){
911 uint16_t ccrc;
912 data = read(timeout);
913 if(data!=-1){
914 ccrc = data << 8;
915 data = read(timeout);
916 if(data!=-1){
917 ccrc |= data;
918 if(crc_get()==ccrc){
919 S3mode = val1;
920 S4mode = val2;
921 S5mode = val3;
922 return true;
923 }
924 }
925 }
926 }
927 }while(trys--);
928
929 return false;
930}
931
932bool RoboClaw::SetDeadBand(uint8_t address, uint8_t Min, uint8_t Max){
933 return write_n(4,address,SETDEADBAND,Min,Max);
934}
935
936bool RoboClaw::GetDeadBand(uint8_t address, uint8_t &Min, uint8_t &Max){
937 bool valid;
938 uint16_t value = Read2(address,GETDEADBAND,&valid);
939 if(valid){
940 Min = value>>8;
941 Max = value;
942 }
943 return valid;
944}
945
946bool RoboClaw::ReadEncoders(uint8_t address,uint32_t &enc1,uint32_t &enc2){
947 bool valid = read_n(2,address,GETENCODERS,&enc1,&enc2);
948 return valid;
949}
950
951bool RoboClaw::ReadISpeeds(uint8_t address,uint32_t &ispeed1,uint32_t &ispeed2){
952 bool valid = read_n(2,address,GETISPEEDS,&ispeed1,&ispeed2);
953 return valid;
954}
955
956bool RoboClaw::RestoreDefaults(uint8_t address){
957 return write_n(2,address,RESTOREDEFAULTS);
958}
959
960bool RoboClaw::ReadTemp(uint8_t address, uint16_t &temp){
961 bool valid;
962 temp = Read2(address,GETTEMP,&valid);
963 return valid;
964}
965
966bool RoboClaw::ReadTemp2(uint8_t address, uint16_t &temp){
967 bool valid;
968 temp = Read2(address,GETTEMP2,&valid);
969 return valid;
970}
971
972uint16_t RoboClaw::ReadError(uint8_t address,bool *valid){
973 return Read2(address,GETERROR,valid);
974}
975
976bool RoboClaw::ReadEncoderModes(uint8_t address, uint8_t &M1mode, uint8_t &M2mode){
977 bool valid;
978 uint16_t value = Read2(address,GETENCODERMODE,&valid);
979 if(valid){
980 M1mode = value>>8;
981 M2mode = value;
982 }
983 return valid;
984}
985
986bool RoboClaw::SetM1EncoderMode(uint8_t address,uint8_t mode){
987 return write_n(3,address,SETM1ENCODERMODE,mode);
988}
989
990bool RoboClaw::SetM2EncoderMode(uint8_t address,uint8_t mode){
991 return write_n(3,address,SETM2ENCODERMODE,mode);
992}
993
994bool RoboClaw::WriteNVM(uint8_t address){
995 return write_n(6,address,WRITENVM, SetDWORDval(0xE22EAB7A) );
996}
997
998bool RoboClaw::ReadNVM(uint8_t address){
999 return write_n(2,address,READNVM);
1000}
1001
1002bool RoboClaw::SetConfig(uint8_t address, uint16_t config){
1003 return write_n(4,address,SETCONFIG,SetWORDval(config));
1004}
1005
1006bool RoboClaw::GetConfig(uint8_t address, uint16_t &config){
1007 bool valid;
1008 uint16_t value = Read2(address,GETCONFIG,&valid);
1009 if(valid){
1010 config = value;
1011 }
1012 return valid;
1013}
1014
1015bool RoboClaw::SetM1MaxCurrent(uint8_t address,uint32_t max){
1016 return write_n(10,address,SETM1MAXCURRENT,SetDWORDval(max),SetDWORDval(0));
1017}
1018
1019bool RoboClaw::SetM2MaxCurrent(uint8_t address,uint32_t max){
1020 return write_n(10,address,SETM2MAXCURRENT,SetDWORDval(max),SetDWORDval(0));
1021}
1022
1023bool RoboClaw::ReadM1MaxCurrent(uint8_t address,uint32_t &max){
1024 uint32_t tmax,dummy;
1025 bool valid = read_n(2,address,GETM1MAXCURRENT,&tmax,&dummy);
1026 if(valid)
1027 max = tmax;
1028 return valid;
1029}
1030
1031bool RoboClaw::ReadM2MaxCurrent(uint8_t address,uint32_t &max){
1032 uint32_t tmax,dummy;
1033 bool valid = read_n(2,address,GETM2MAXCURRENT,&tmax,&dummy);
1034 if(valid)
1035 max = tmax;
1036 return valid;
1037}
1038
1039bool RoboClaw::SetPWMMode(uint8_t address, uint8_t mode){
1040 return write_n(3,address,SETPWMMODE,mode);
1041}
1042
1043bool RoboClaw::GetPWMMode(uint8_t address, uint8_t &mode){
1044 bool valid;
1045 uint8_t value = Read1(address,GETPWMMODE,&valid);
1046 if(valid){
1047 mode = value;
1048 }
1049 return valid;
1050}
1051