MatrixMiniR4 1.2.2
Matrix Mini R4 Arduino Library API Documentation
Loading...
Searching...
No Matches
MiniR4DriveDC.h
Go to the documentation of this file.
1
6
7#ifndef MINIR4DRIVEDC_H
8#define MINIR4DRIVEDC_H
9
10#include "MMLower.h"
11
12#define IMU_idle_Period 30
13#define Drive_retry_Period 10
14
15#ifdef ENABLE_DRIVEDC_BRAKE_DELAY
16 #ifndef DRIVEDC_BRAKE_DELAY_MS
17 #define DRIVEDC_BRAKE_DELAY_MS 110
18 #endif
19#endif
20
29template < uint8_t ID > class MiniR4DriveDC {
30 public: MiniR4DriveDC() {
31 _id = ID;
32 }
33
49 uint8_t begin(uint8_t leftMotorID, uint8_t rightMotorID, bool isLeftReverse, bool isRightReverse) {
50 _id_left = leftMotorID;
51 _id_right = rightMotorID;
52 _dir_left = (isLeftReverse) ? MMLower::DIR::REVERSE : MMLower::DIR::FORWARD;
53 _dir_right = (isRightReverse) ? MMLower::DIR::REVERSE : MMLower::DIR::FORWARD;
54 uint8_t idx = 0;
55
56 MMLower::Drive_RESULT result = mmL.Set_Drive2Motor_PARAM(_id_left, _id_right, _dir_left, _dir_right, _id);
57
58 while (result != MMLower::Drive_RESULT::OK) {
59 delay(100);
60 result = mmL.Set_Drive2Motor_PARAM(_id_left, _id_right, _dir_left, _dir_right, _id);
61 if (result == MMLower::Drive_RESULT::ERROR_Drive_Param) {
62 idx++;
63 if (idx > IMU_idle_Period) {
64 return 0x08;
65 }
66 } else if (result == MMLower::Drive_RESULT::OK) {
67 return 0x00;
68 } else if (result == MMLower::Drive_RESULT::ERROR_Drive_Param) {
69 idx++;
70 if (idx > IMU_idle_Period) {
71 return 0x09;
72 }
73 }
74 }
75
76 return 0x00;
77
78 }
79
89 // uint8_t set_Drive_Param(uint8_t m1_no, uint8_t m2_no, bool m1_dir, bool m2_dir) {
90 // _id_left = m1_no;
91 // _id_right = m2_no;
92 // _dir_left = (m1_dir) ? MMLower::DIR::REVERSE : MMLower::DIR::FORWARD;
93 // _dir_right = (m2_dir) ? MMLower::DIR::REVERSE : MMLower::DIR::FORWARD;
94 // uint8_t idx = 0;
95
96 // MMLower::Drive_RESULT result = mmL.Set_Drive2Motor_PARAM(_id_left, _id_right, _dir_left, _dir_right, _id);
97
98 // while (result != MMLower::Drive_RESULT::OK) {
99 // delay(100);
100 // result = mmL.Set_Drive2Motor_PARAM(_id_left, _id_right, _dir_left, _dir_right, _id);
101 // if (result == MMLower::Drive_RESULT::ERROR_Drive_Param) {
102 // idx++;
103 // if (idx > IMU_idle_Period) {
104 // return 0x08;
105 // }
106 // } else if (result == MMLower::Drive_RESULT::OK) {
107 // return 0x00;
108 // } else if (result == MMLower::Drive_RESULT::ERROR_Drive_Param) {
109 // idx++;
110 // if (idx > IMU_idle_Period) {
111 // return 0x09;
112 // }
113 // }
114 // }
115
116 // return 0x00;
117 // }
118
127 bool setMoveSyncPID(float kp, float ki, float kd) {
128 MMLower::RESULT result = mmL.Set_Drive_MoveSync_PID(kp, ki, kd, _id);
129
130 if (result != MMLower::RESULT::OK) {
131 delay(2);
132 result = mmL.Set_Drive_MoveSync_PID(kp, ki, kd, _id);
133 }
134
135 return (result == MMLower::RESULT::OK);
136 }
137
146 bool setMoveGyroPID(float kp, float ki, float kd) {
147 MMLower::RESULT result = mmL.Set_Drive_MoveGyro_PID(kp, ki, kd, _id);
148
149 if (result != MMLower::RESULT::OK) {
150 delay(2);
151 result = mmL.Set_Drive_MoveGyro_PID(kp, ki, kd, _id);
152 }
153
154 return (result == MMLower::RESULT::OK);
155 }
156
165 bool setTurnGyroPID(float kp, float ki, float kd) {
166 MMLower::RESULT result = mmL.Set_Drive_MoveTurn_PID(kp, ki, kd, _id);
167
168 if (result != MMLower::RESULT::OK) {
169 delay(2);
170 result = mmL.Set_Drive_MoveTurn_PID(kp, ki, kd, _id);
171 }
172
173 return (result == MMLower::RESULT::OK);
174 }
175
182 // bool setMotorType(uint8_t typeXC) {
183 // MMLower::Drive_RESULT result = mmL.Set_Drive_Motor_Type(_id, typeXC);
184
185 // if (result != MMLower::Drive_RESULT::OK) {
186 // delay(2);
187 // result = mmL.Set_Drive_Motor_Type(_id, typeXC);
188 // }
189
190 // return (result == MMLower::Drive_RESULT::OK);
191 // }
192
203 uint8_t Move(int16_t power_left, int16_t power_right) {
204
205 MMLower::Drive_RESULT result = mmL.Set_Drive_Move_Func(power_left, power_right, _id);
206
207 uint8_t idx = 0;
208 while (result != MMLower::Drive_RESULT::OK) {
209 delay(2);
210 result = mmL.Set_Drive_Move_Func(power_left, power_right, _id);
211 idx++;
212 if (result == MMLower::Drive_RESULT::ERROR_Drive_Define) {
213 if (idx >= Drive_retry_Period)
214 return 0x07;
215 } else if (result != MMLower::Drive_RESULT::OK) {
216 if (idx >= Drive_retry_Period)
217 return 0x01;
218 } else {
219 return 0x00;
220 }
221 }
222 }
223
237 uint8_t MoveDegs(int16_t power_left, int16_t power_right, uint16_t degree, bool brake, bool async = false) {
238 MMLower::Drive_RESULT result = mmL.Set_Drive_Move_Degs(power_left, power_right, degree, brake, async, _id);
239
240 uint8_t idx = 0;
241 while (result != MMLower::Drive_RESULT::OK) {
242 delay(2);
243 result = mmL.Set_Drive_Move_Degs(power_left, power_right, degree, brake, async, _id);
244 idx++;
245 if (result == MMLower::Drive_RESULT::ERROR_Drive_Define) {
246 if (idx >= Drive_retry_Period)
247 return 0x07;
248 } else if (result != MMLower::Drive_RESULT::OK) {
249 if (idx >= Drive_retry_Period)
250 return 0x01;
251 } else {
252 break;
253 }
254 }
255
256 if (async == false) {
257 bool stats[2];
258 stats[0] = true;
259 while (stats[0]) {
260 delay(50);
261 mmL.Get_Drive_isTaskDone(_id, stats);
262 }
263
264 #ifdef ENABLE_DRIVEDC_BRAKE_DELAY
265 delay(DRIVEDC_BRAKE_DELAY_MS); //Give motor some time to stop.
266 #endif
267 }
268 return 0x00;
269 }
270
284 uint8_t MoveTime(int16_t power_left, int16_t power_right, float Time_S, bool brake, bool async = false) {
285 Time_S = Time_S * 1000; // to milliseconds
286 MMLower::Drive_RESULT result = mmL.Set_Drive_Move_Time(power_left, power_right, Time_S, brake, async, _id);
287
288 uint8_t idx = 0;
289 while (result != MMLower::Drive_RESULT::OK) {
290 delay(2);
291 result = mmL.Set_Drive_Move_Time(power_left, power_right, Time_S, brake, async, _id);
292 idx++;
293 if (result == MMLower::Drive_RESULT::ERROR_Drive_Define) {
294 if (idx >= Drive_retry_Period)
295 return 0x07;
296 } else if (result != MMLower::Drive_RESULT::OK) {
297 if (idx >= Drive_retry_Period)
298 return 0x01;
299 } else {
300 break;
301 }
302 }
303
304 if (async == false) {
305 bool stats[2];
306 stats[0] = true;
307 while (stats[0]) {
308 delay(50);
309 mmL.Get_Drive_isTaskDone(_id, stats);
310 }
311
312 #ifdef ENABLE_DRIVEDC_BRAKE_DELAY
313 delay(DRIVEDC_BRAKE_DELAY_MS); //Give motor some time to stop.
314 #endif
315 }
316 return 0x00;
317 }
318
329 uint8_t MoveSync(int16_t power_left, int16_t power_right) {
330 MMLower::Drive_RESULT result = mmL.Set_Drive_MoveSync_Func(power_left, power_right, _id);
331
332 uint8_t idx = 0;
333 while (result != MMLower::Drive_RESULT::OK) {
334 delay(2);
335 result = mmL.Set_Drive_MoveSync_Func(power_left, power_right, _id);
336 idx++;
337 if (result == MMLower::Drive_RESULT::ERROR_Drive_Define) {
338 if (idx >= Drive_retry_Period)
339 return 0x07;
340 } else if (result != MMLower::Drive_RESULT::OK) {
341 if (idx >= Drive_retry_Period)
342 return 0x01;
343 } else {
344 return 0x00;
345 }
346
347 }
348 }
349
363 uint8_t MoveSyncDegs(int16_t power_left, int16_t power_right, uint16_t degree, bool brake, bool async = false) {
364 MMLower::Drive_RESULT result = mmL.Set_Drive_MoveSync_Degs(power_left, power_right, degree, brake, async, _id);
365
366 uint8_t idx = 0;
367 while (result != MMLower::Drive_RESULT::OK) {
368 delay(2);
369 result = mmL.Set_Drive_MoveSync_Degs(power_left, power_right, degree, brake, async, _id);
370 idx++;
371 if (result == MMLower::Drive_RESULT::ERROR_Drive_Define) {
372 if (idx >= Drive_retry_Period)
373 return 0x07;
374 } else if (result != MMLower::Drive_RESULT::OK) {
375 if (idx >= Drive_retry_Period)
376 return 0x01;
377 } else {
378 break;
379 }
380 }
381
382 if (async == false) {
383 bool stats[2];
384 stats[0] = true;
385 while (stats[0]) {
386 delay(10);
387 mmL.Get_Drive_isTaskDone(_id, stats);
388 }
389
390 #ifdef ENABLE_DRIVEDC_BRAKE_DELAY
391 delay(DRIVEDC_BRAKE_DELAY_MS); //Give motor some time to stop.
392 #endif
393 }
394
395 return 0x00;
396
397 }
398
412 uint8_t MoveSyncTime(int16_t power_left, int16_t power_right, float Time_S, bool brake, bool async = false) {
413 Time_S = Time_S * 1000; // to milliseconds
414 MMLower::Drive_RESULT result = mmL.Set_Drive_MoveSync_Time(power_left, power_right, Time_S, brake, async, _id);
415
416 uint8_t idx = 0;
417 while (result != MMLower::Drive_RESULT::OK) {
418 delay(2);
419 result = mmL.Set_Drive_MoveSync_Time(power_left, power_right, Time_S, brake, async, _id);
420 idx++;
421 if (result == MMLower::Drive_RESULT::ERROR_Drive_Define) {
422 if (idx >= Drive_retry_Period)
423 return 0x07;
424 } else if (result != MMLower::Drive_RESULT::OK) {
425 if (idx >= Drive_retry_Period)
426 return 0x01;
427 } else {
428 break;
429 }
430 }
431
432 if (async == false) {
433 bool stats[2];
434 stats[0] = true;
435 while (stats[0]) {
436 delay(10);
437 mmL.Get_Drive_isTaskDone(_id, stats);
438 }
439
440 #ifdef ENABLE_DRIVEDC_BRAKE_DELAY
441 delay(DRIVEDC_BRAKE_DELAY_MS); //Give motor some time to stop.
442 #endif
443 }
444
445 return 0x00;
446
447 }
448
458 uint8_t MoveGyro(int16_t power, int16_t Target) {
459 MMLower::Drive_RESULT result = mmL.Set_Drive_MoveGyro_Func(power, Target, _id);
460
461 uint8_t idx = 0;
462 while (result != MMLower::Drive_RESULT::OK) {
463 delay(2);
464 result = mmL.Set_Drive_MoveGyro_Func(power, Target, _id);
465 idx++;
466 if (result == MMLower::Drive_RESULT::ERROR_Drive_Define) {
467 if (idx >= Drive_retry_Period)
468 return 0x07;
469 } else if (result != MMLower::Drive_RESULT::OK) {
470 if (idx >= Drive_retry_Period)
471 return 0x01;
472 } else {
473 return 0x00;
474 }
475
476 }
477 }
478
491 uint8_t MoveGyroDegs(int16_t power_X, int16_t Target_D, uint16_t degree, bool brake, bool async = false) {
492 MMLower::Drive_RESULT result = mmL.Set_Drive_MoveGyro_Degs(power_X, Target_D, degree, brake, async, _id);
493
494 uint8_t idx = 0;
495 while (result != MMLower::Drive_RESULT::OK) {
496 delay(2);
497 result = mmL.Set_Drive_MoveGyro_Degs(power_X, Target_D, degree, brake, async, _id);
498 idx++;
499 if (result == MMLower::Drive_RESULT::ERROR_Drive_Define) {
500 if (idx >= Drive_retry_Period)
501 return 0x07;
502 } else if (result != MMLower::Drive_RESULT::OK) {
503 if (idx >= Drive_retry_Period)
504 return 0x01;
505 } else {
506 return 0x00;
507 }
508 }
509
510 if (async == false) {
511 bool stats[2];
512 stats[0] = true;
513 while (stats[0]) {
514 delay(10);
515 mmL.Get_Drive_isTaskDone(_id, stats);
516 }
517
518 #ifdef ENABLE_DRIVEDC_BRAKE_DELAY
519 delay(DRIVEDC_BRAKE_DELAY_MS); //Give motor some time to stop.
520 #endif
521 }
522
523 return 0x00;
524
525 }
526
539 uint8_t MoveGyroTime(int16_t power, int16_t Target, float Time_S, bool brake, bool async = false) {
540 Time_S = Time_S * 1000; // to milliseconds
541 MMLower::Drive_RESULT result = mmL.Set_Drive_MoveGyro_Time(power, Target, Time_S, brake, async, _id);
542
543 uint8_t idx = 0;
544 while (result != MMLower::Drive_RESULT::OK) {
545 delay(2);
546 result = mmL.Set_Drive_MoveGyro_Time(power, Target, Time_S, brake, async, _id);
547 idx++;
548 if (result == MMLower::Drive_RESULT::ERROR_Drive_Define) {
549 if (idx >= Drive_retry_Period)
550 return 0x07;
551 } else if (result != MMLower::Drive_RESULT::OK) {
552 if (idx >= Drive_retry_Period)
553 return 0x01;
554 } else {
555 return 0x00;
556 }
557 }
558
559 if (async == false) {
560 bool stats[2];
561 stats[0] = true;
562 while (stats[0]) {
563 delay(10);
564 mmL.Get_Drive_isTaskDone(_id, stats);
565 }
566
567 #ifdef ENABLE_DRIVEDC_BRAKE_DELAY
568 delay(DRIVEDC_BRAKE_DELAY_MS); //Give motor some time to stop.
569 #endif
570 }
571
572 return 0x00;
573 }
574
587 uint8_t TurnGyro(int16_t power, int16_t Target_D, uint8_t mode, bool brake, bool async = false) {
588 MMLower::Drive_RESULT result = mmL.Set_Drive_TurnGyro(power, Target_D, mode, brake, async, _id);
589
590 if (result != MMLower::Drive_RESULT::OK) {
591 delay(1);
592 result = mmL.Set_Drive_TurnGyro(power, Target_D, mode, brake, async, _id);
593 }
594
595 if (result == MMLower::Drive_RESULT::ERROR_Drive_Define) {
596 return 0x07;
597 } else if (result != MMLower::Drive_RESULT::OK) {
598 return 0x01;
599 }
600
601 if (async == false) {
602 bool stats[2];
603 stats[0] = true;
604 while (stats[0]) {
605 delay(10);
606 mmL.Get_Drive_isTaskDone(_id, stats);
607 }
608
609 #ifdef ENABLE_DRIVEDC_BRAKE_DELAY
610 delay(DRIVEDC_BRAKE_DELAY_MS); //Give motor some time to stop.
611 #endif
612 }
613
614 return 0x00;
615 }
616
622 int32_t getCounter(void) {
623 int32_t counter = 0;
624 MMLower::Drive_RESULT result = mmL.Get_Drive_EncoderCounter(_id, counter);
625
626 if (result != MMLower::Drive_RESULT::OK) {
627 delay(1);
628 result = mmL.Get_Drive_EncoderCounter(_id, counter);
629 }
630
631 return counter;
632 }
633
639 int32_t getDegrees(void) {
640 int32_t degrees = 0;
641 MMLower::Drive_RESULT result = mmL.Get_Drive_Degrees(_id, degrees);
642
643 if (result != MMLower::Drive_RESULT::OK) {
644 delay(1);
645 result = mmL.Get_Drive_Degrees(_id, degrees);
646 }
647
648 return degrees;
649 }
650
656 uint8_t resetCounter(void) {
657 MMLower::Drive_RESULT result = mmL.Set_Drive_Reset_Count(_id, true);
658
659 if (result == MMLower::Drive_RESULT::ERROR_Drive_Define) {
660 return 0x07;
661 } else if (result == MMLower::Drive_RESULT::OK) {
662 return 0x00;
663 } else {
664 return 0x01;
665 }
666 }
667
676 uint8_t brake(bool brakeType) {
677 MMLower::Drive_RESULT result = mmL.Set_Drive_Brake(brakeType, _id);
678
679 if (result == MMLower::Drive_RESULT::ERROR_Drive_Define) {
680 return 0x07;
681 } else if (result == MMLower::Drive_RESULT::OK) {
682 return 0x00;
683 } else {
684 return 0x01;
685 }
686 }
687
693 bool isPrevTaskDone(void) {
694 bool stsu[2];
695
696 MMLower::Drive_RESULT result = mmL.Get_Drive_isTaskDone(_id, stsu);
697 return stsu[0] ? false : true;
698
699 }
700
701 private: uint8_t _id_left,
702 _id_right;
703 uint8_t _id;
704 MMLower::DIR _dir_left,
705 _dir_right;
706};
707
708#endif // MINIR4DRIVEDC_H
mmL
MMLower mmL(8, 9, 57600)
MMLower.h
Handling the Lower MCU (STM32) communication.
Drive_retry_Period
#define Drive_retry_Period
Definition MiniR4DriveDC.h:13
IMU_idle_Period
#define IMU_idle_Period
Definition MiniR4DriveDC.h:12
MMLower::Drive_RESULT
Drive_RESULT
Definition MMLower.h:306
MiniR4DriveDC::setTurnGyroPID
bool setTurnGyroPID(float kp, float ki, float kd)
Sets the PID parameters for DriveDC TurnGyro.
Definition MiniR4DriveDC.h:165
MiniR4DriveDC::Move
uint8_t Move(int16_t power_left, int16_t power_right)
Sets the Motor Type for DriveDC. (Unuse)
Definition MiniR4DriveDC.h:203
MiniR4DriveDC::getCounter
int32_t getCounter(void)
Gets the estimated rotation counter of the drivebase. (not degrees)
Definition MiniR4DriveDC.h:622
MiniR4DriveDC::MoveGyroDegs
uint8_t MoveGyroDegs(int16_t power_X, int16_t Target_D, uint16_t degree, bool brake, bool async=false)
Sets the power level with gyro guidance for specific degrees.
Definition MiniR4DriveDC.h:491
MiniR4DriveDC::MoveSyncDegs
uint8_t MoveSyncDegs(int16_t power_left, int16_t power_right, uint16_t degree, bool brake, bool async=false)
Sets the power level of the DC motor with synchronization for specific degrees.
Definition MiniR4DriveDC.h:363
MiniR4DriveDC::brake
uint8_t brake(bool brakeType)
Sets the brake mode for the DC motor.
Definition MiniR4DriveDC.h:676
MiniR4DriveDC::MoveSync
uint8_t MoveSync(int16_t power_left, int16_t power_right)
Sets the power level of the DC motor with synchronization.
Definition MiniR4DriveDC.h:329
MiniR4DriveDC::MoveGyro
uint8_t MoveGyro(int16_t power, int16_t Target)
Sets the power level with gyro guidance.
Definition MiniR4DriveDC.h:458
MiniR4DriveDC::setMoveSyncPID
bool setMoveSyncPID(float kp, float ki, float kd)
Sets the DriveDC motor parameters (Unuse)
Definition MiniR4DriveDC.h:127
MiniR4DriveDC::resetCounter
uint8_t resetCounter(void)
Resets the drivebase encoder counter/degrees to zero.
Definition MiniR4DriveDC.h:656
MiniR4DriveDC::MoveDegs
uint8_t MoveDegs(int16_t power_left, int16_t power_right, uint16_t degree, bool brake, bool async=false)
Sets the power level of the DC motor for a specific degree.
Definition MiniR4DriveDC.h:237
MiniR4DriveDC::getDegrees
int32_t getDegrees(void)
Gets the estimated degrees of the drivebase.
Definition MiniR4DriveDC.h:639
MiniR4DriveDC::MoveGyroTime
uint8_t MoveGyroTime(int16_t power, int16_t Target, float Time_S, bool brake, bool async=false)
Sets the power level with gyro guidance for specific time.
Definition MiniR4DriveDC.h:539
MiniR4DriveDC::MoveTime
uint8_t MoveTime(int16_t power_left, int16_t power_right, float Time_S, bool brake, bool async=false)
Sets the power level of the DC motor for a specific time period.
Definition MiniR4DriveDC.h:284
MiniR4DriveDC::MoveSyncTime
uint8_t MoveSyncTime(int16_t power_left, int16_t power_right, float Time_S, bool brake, bool async=false)
Sets the power level of the DC motor with synchronization for specific time.
Definition MiniR4DriveDC.h:412
MiniR4DriveDC::begin
uint8_t begin(uint8_t leftMotorID, uint8_t rightMotorID, bool isLeftReverse, bool isRightReverse)
Initializes the DC motor settings.
Definition MiniR4DriveDC.h:49
MiniR4DriveDC::isPrevTaskDone
bool isPrevTaskDone(void)
Checks if the previous task has completed.
Definition MiniR4DriveDC.h:693
MiniR4DriveDC::setMoveGyroPID
bool setMoveGyroPID(float kp, float ki, float kd)
Sets the PID parameters for DriveDC MoveGyro.
Definition MiniR4DriveDC.h:146
MiniR4DriveDC::TurnGyro
uint8_t TurnGyro(int16_t power, int16_t Target_D, uint8_t mode, bool brake, bool async=false)
Turns the robot to a specific gyro heading.
Definition MiniR4DriveDC.h:587