MatrixMiniR4 1.1.9
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
88 bool setMoveSyncPID(float kp, float ki, float kd) {
89 MMLower::RESULT result = mmL.Set_Drive_MoveSync_PID(kp, ki, kd, _id);
90
91 if (result != MMLower::RESULT::OK) {
92 delay(2);
93 result = mmL.Set_Drive_MoveSync_PID(kp, ki, kd, _id);
94 }
95
96 return (result == MMLower::RESULT::OK);
97 }
98
107 bool setMoveGyroPID(float kp, float ki, float kd) {
108 MMLower::RESULT result = mmL.Set_Drive_MoveGyro_PID(kp, ki, kd, _id);
109
110 if (result != MMLower::RESULT::OK) {
111 delay(2);
112 result = mmL.Set_Drive_MoveGyro_PID(kp, ki, kd, _id);
113 }
114
115 return (result == MMLower::RESULT::OK);
116 }
117
126 bool setTurnGyroPID(float kp, float ki, float kd) {
127 MMLower::RESULT result = mmL.Set_Drive_MoveTurn_PID(kp, ki, kd, _id);
128
129 if (result != MMLower::RESULT::OK) {
130 delay(2);
131 result = mmL.Set_Drive_MoveTurn_PID(kp, ki, kd, _id);
132 }
133
134 return (result == MMLower::RESULT::OK);
135 }
136
143 // bool setMotorType(uint8_t typeXC) {
144 // MMLower::Drive_RESULT result = mmL.Set_Drive_Motor_Type(_id, typeXC);
145
146 // if (result != MMLower::Drive_RESULT::OK) {
147 // delay(2);
148 // result = mmL.Set_Drive_Motor_Type(_id, typeXC);
149 // }
150
151 // return (result == MMLower::Drive_RESULT::OK);
152 // }
153
164 uint8_t Move(int16_t power_left, int16_t power_right) {
165
166 MMLower::Drive_RESULT result = mmL.Set_Drive_Move_Func(power_left, power_right, _id);
167
168 uint8_t idx = 0;
169 while (result != MMLower::Drive_RESULT::OK) {
170 delay(2);
171 result = mmL.Set_Drive_Move_Func(power_left, power_right, _id);
172 idx++;
173 if (result == MMLower::Drive_RESULT::ERROR_Drive_Define) {
174 if (idx >= Drive_retry_Period)
175 return 0x07;
176 } else if (result != MMLower::Drive_RESULT::OK) {
177 if (idx >= Drive_retry_Period)
178 return 0x01;
179 } else {
180 return 0x00;
181 }
182 }
183 }
184
198 uint8_t MoveDegs(int16_t power_left, int16_t power_right, uint16_t degree, bool brake, bool async = false) {
199 MMLower::Drive_RESULT result = mmL.Set_Drive_Move_Degs(power_left, power_right, degree, brake, async, _id);
200
201 uint8_t idx = 0;
202 while (result != MMLower::Drive_RESULT::OK) {
203 delay(2);
204 result = mmL.Set_Drive_Move_Degs(power_left, power_right, degree, brake, async, _id);
205 idx++;
206 if (result == MMLower::Drive_RESULT::ERROR_Drive_Define) {
207 if (idx >= Drive_retry_Period)
208 return 0x07;
209 } else if (result != MMLower::Drive_RESULT::OK) {
210 if (idx >= Drive_retry_Period)
211 return 0x01;
212 } else {
213 break;
214 }
215 }
216
217 if (async == false) {
218 bool stats[2];
219 stats[0] = true;
220 while (stats[0]) {
221 delay(50);
222 mmL.Get_Drive_isTaskDone(_id, stats);
223 }
224
225 #ifdef ENABLE_DRIVEDC_BRAKE_DELAY
226 delay(DRIVEDC_BRAKE_DELAY_MS); //Give motor some time to stop.
227 #endif
228 }
229 return 0x00;
230 }
231
245 uint8_t MoveTime(int16_t power_left, int16_t power_right, float Time_S, bool brake, bool async = false) {
246 Time_S = Time_S * 1000; // to milliseconds
247 MMLower::Drive_RESULT result = mmL.Set_Drive_Move_Time(power_left, power_right, Time_S, brake, async, _id);
248
249 uint8_t idx = 0;
250 while (result != MMLower::Drive_RESULT::OK) {
251 delay(2);
252 result = mmL.Set_Drive_Move_Time(power_left, power_right, Time_S, brake, async, _id);
253 idx++;
254 if (result == MMLower::Drive_RESULT::ERROR_Drive_Define) {
255 if (idx >= Drive_retry_Period)
256 return 0x07;
257 } else if (result != MMLower::Drive_RESULT::OK) {
258 if (idx >= Drive_retry_Period)
259 return 0x01;
260 } else {
261 break;
262 }
263 }
264
265 if (async == false) {
266 bool stats[2];
267 stats[0] = true;
268 while (stats[0]) {
269 delay(50);
270 mmL.Get_Drive_isTaskDone(_id, stats);
271 }
272
273 #ifdef ENABLE_DRIVEDC_BRAKE_DELAY
274 delay(DRIVEDC_BRAKE_DELAY_MS); //Give motor some time to stop.
275 #endif
276 }
277 return 0x00;
278 }
279
290 uint8_t MoveSync(int16_t power_left, int16_t power_right) {
291 MMLower::Drive_RESULT result = mmL.Set_Drive_MoveSync_Func(power_left, power_right, _id);
292
293 uint8_t idx = 0;
294 while (result != MMLower::Drive_RESULT::OK) {
295 delay(2);
296 result = mmL.Set_Drive_MoveSync_Func(power_left, power_right, _id);
297 idx++;
298 if (result == MMLower::Drive_RESULT::ERROR_Drive_Define) {
299 if (idx >= Drive_retry_Period)
300 return 0x07;
301 } else if (result != MMLower::Drive_RESULT::OK) {
302 if (idx >= Drive_retry_Period)
303 return 0x01;
304 } else {
305 return 0x00;
306 }
307
308 }
309 }
310
324 uint8_t MoveSyncDegs(int16_t power_left, int16_t power_right, uint16_t degree, bool brake, bool async = false) {
325 MMLower::Drive_RESULT result = mmL.Set_Drive_MoveSync_Degs(power_left, power_right, degree, brake, async, _id);
326
327 uint8_t idx = 0;
328 while (result != MMLower::Drive_RESULT::OK) {
329 delay(2);
330 result = mmL.Set_Drive_MoveSync_Degs(power_left, power_right, degree, brake, async, _id);
331 idx++;
332 if (result == MMLower::Drive_RESULT::ERROR_Drive_Define) {
333 if (idx >= Drive_retry_Period)
334 return 0x07;
335 } else if (result != MMLower::Drive_RESULT::OK) {
336 if (idx >= Drive_retry_Period)
337 return 0x01;
338 } else {
339 break;
340 }
341 }
342
343 if (async == false) {
344 bool stats[2];
345 stats[0] = true;
346 while (stats[0]) {
347 delay(10);
348 mmL.Get_Drive_isTaskDone(_id, stats);
349 }
350
351 #ifdef ENABLE_DRIVEDC_BRAKE_DELAY
352 delay(DRIVEDC_BRAKE_DELAY_MS); //Give motor some time to stop.
353 #endif
354 }
355
356 return 0x00;
357
358 }
359
373 uint8_t MoveSyncTime(int16_t power_left, int16_t power_right, float Time_S, bool brake, bool async = false) {
374 Time_S = Time_S * 1000; // to milliseconds
375 MMLower::Drive_RESULT result = mmL.Set_Drive_MoveSync_Time(power_left, power_right, Time_S, brake, async, _id);
376
377 uint8_t idx = 0;
378 while (result != MMLower::Drive_RESULT::OK) {
379 delay(2);
380 result = mmL.Set_Drive_MoveSync_Time(power_left, power_right, Time_S, brake, async, _id);
381 idx++;
382 if (result == MMLower::Drive_RESULT::ERROR_Drive_Define) {
383 if (idx >= Drive_retry_Period)
384 return 0x07;
385 } else if (result != MMLower::Drive_RESULT::OK) {
386 if (idx >= Drive_retry_Period)
387 return 0x01;
388 } else {
389 break;
390 }
391 }
392
393 if (async == false) {
394 bool stats[2];
395 stats[0] = true;
396 while (stats[0]) {
397 delay(10);
398 mmL.Get_Drive_isTaskDone(_id, stats);
399 }
400
401 #ifdef ENABLE_DRIVEDC_BRAKE_DELAY
402 delay(DRIVEDC_BRAKE_DELAY_MS); //Give motor some time to stop.
403 #endif
404 }
405
406 return 0x00;
407
408 }
409
419 uint8_t MoveGyro(int16_t power, int16_t Target) {
420 MMLower::Drive_RESULT result = mmL.Set_Drive_MoveGyro_Func(power, Target, _id);
421
422 uint8_t idx = 0;
423 while (result != MMLower::Drive_RESULT::OK) {
424 delay(2);
425 result = mmL.Set_Drive_MoveGyro_Func(power, Target, _id);
426 idx++;
427 if (result == MMLower::Drive_RESULT::ERROR_Drive_Define) {
428 if (idx >= Drive_retry_Period)
429 return 0x07;
430 } else if (result != MMLower::Drive_RESULT::OK) {
431 if (idx >= Drive_retry_Period)
432 return 0x01;
433 } else {
434 return 0x00;
435 }
436
437 }
438 }
439
452 uint8_t MoveGyroDegs(int16_t power_X, int16_t Target_D, uint16_t degree, bool brake, bool async = false) {
453 MMLower::Drive_RESULT result = mmL.Set_Drive_MoveGyro_Degs(power_X, Target_D, degree, brake, async, _id);
454
455 uint8_t idx = 0;
456 while (result != MMLower::Drive_RESULT::OK) {
457 delay(2);
458 result = mmL.Set_Drive_MoveGyro_Degs(power_X, Target_D, degree, brake, async, _id);
459 idx++;
460 if (result == MMLower::Drive_RESULT::ERROR_Drive_Define) {
461 if (idx >= Drive_retry_Period)
462 return 0x07;
463 } else if (result != MMLower::Drive_RESULT::OK) {
464 if (idx >= Drive_retry_Period)
465 return 0x01;
466 } else {
467 return 0x00;
468 }
469 }
470
471 if (async == false) {
472 bool stats[2];
473 stats[0] = true;
474 while (stats[0]) {
475 delay(10);
476 mmL.Get_Drive_isTaskDone(_id, stats);
477 }
478
479 #ifdef ENABLE_DRIVEDC_BRAKE_DELAY
480 delay(DRIVEDC_BRAKE_DELAY_MS); //Give motor some time to stop.
481 #endif
482 }
483
484 return 0x00;
485
486 }
487
500 uint8_t MoveGyroTime(int16_t power, int16_t Target, float Time_S, bool brake, bool async = false) {
501 Time_S = Time_S * 1000; // to milliseconds
502 MMLower::Drive_RESULT result = mmL.Set_Drive_MoveGyro_Time(power, Target, Time_S, brake, async, _id);
503
504 uint8_t idx = 0;
505 while (result != MMLower::Drive_RESULT::OK) {
506 delay(2);
507 result = mmL.Set_Drive_MoveGyro_Time(power, Target, Time_S, brake, async, _id);
508 idx++;
509 if (result == MMLower::Drive_RESULT::ERROR_Drive_Define) {
510 if (idx >= Drive_retry_Period)
511 return 0x07;
512 } else if (result != MMLower::Drive_RESULT::OK) {
513 if (idx >= Drive_retry_Period)
514 return 0x01;
515 } else {
516 return 0x00;
517 }
518 }
519
520 if (async == false) {
521 bool stats[2];
522 stats[0] = true;
523 while (stats[0]) {
524 delay(10);
525 mmL.Get_Drive_isTaskDone(_id, stats);
526 }
527
528 #ifdef ENABLE_DRIVEDC_BRAKE_DELAY
529 delay(DRIVEDC_BRAKE_DELAY_MS); //Give motor some time to stop.
530 #endif
531 }
532
533 return 0x00;
534 }
535
548 uint8_t TurnGyro(int16_t power, int16_t Target_D, uint8_t mode, bool brake, bool async = false) {
549 MMLower::Drive_RESULT result = mmL.Set_Drive_TurnGyro(power, Target_D, mode, brake, async, _id);
550
551 if (result != MMLower::Drive_RESULT::OK) {
552 delay(1);
553 result = mmL.Set_Drive_TurnGyro(power, Target_D, mode, brake, async, _id);
554 }
555
556 if (result == MMLower::Drive_RESULT::ERROR_Drive_Define) {
557 return 0x07;
558 } else if (result != MMLower::Drive_RESULT::OK) {
559 return 0x01;
560 }
561
562 if (async == false) {
563 bool stats[2];
564 stats[0] = true;
565 while (stats[0]) {
566 delay(10);
567 mmL.Get_Drive_isTaskDone(_id, stats);
568 }
569
570 #ifdef ENABLE_DRIVEDC_BRAKE_DELAY
571 delay(DRIVEDC_BRAKE_DELAY_MS); //Give motor some time to stop.
572 #endif
573 }
574
575 return 0x00;
576 }
577
583 int32_t getCounter(void) {
584 int32_t counter = 0;
585 MMLower::Drive_RESULT result = mmL.Get_Drive_EncoderCounter(_id, counter);
586
587 if (result != MMLower::Drive_RESULT::OK) {
588 delay(1);
589 result = mmL.Get_Drive_EncoderCounter(_id, counter);
590 }
591
592 return counter;
593 }
594
600 int32_t getDegrees(void) {
601 int32_t degrees = 0;
602 MMLower::Drive_RESULT result = mmL.Get_Drive_Degrees(_id, degrees);
603
604 if (result != MMLower::Drive_RESULT::OK) {
605 delay(1);
606 result = mmL.Get_Drive_Degrees(_id, degrees);
607 }
608
609 return degrees;
610 }
611
617 uint8_t resetCounter(void) {
618 MMLower::Drive_RESULT result = mmL.Set_Drive_Reset_Count(_id, true);
619
620 if (result == MMLower::Drive_RESULT::ERROR_Drive_Define) {
621 return 0x07;
622 } else if (result == MMLower::Drive_RESULT::OK) {
623 return 0x00;
624 } else {
625 return 0x01;
626 }
627 }
628
637 uint8_t brake(bool brakeType) {
638 MMLower::Drive_RESULT result = mmL.Set_Drive_Brake(brakeType, _id);
639
640 if (result == MMLower::Drive_RESULT::ERROR_Drive_Define) {
641 return 0x07;
642 } else if (result == MMLower::Drive_RESULT::OK) {
643 return 0x00;
644 } else {
645 return 0x01;
646 }
647 }
648
654 bool isPrevTaskDone(void) {
655 bool stsu[2];
656
657 MMLower::Drive_RESULT result = mmL.Get_Drive_isTaskDone(_id, stsu);
658 return stsu[0] ? false : true;
659
660 }
661
662 private: uint8_t _id_left,
663 _id_right;
664 uint8_t _id;
665 MMLower::DIR _dir_left,
666 _dir_right;
667};
668
669#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:126
MiniR4DriveDC::Move
uint8_t Move(int16_t power_left, int16_t power_right)
Sets the Motor Type for DriveDC. (Unuse)
Definition MiniR4DriveDC.h:164
MiniR4DriveDC::getCounter
int32_t getCounter(void)
Gets the estimated rotation counter of the drivebase. (not degrees)
Definition MiniR4DriveDC.h:583
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:452
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:324
MiniR4DriveDC::brake
uint8_t brake(bool brakeType)
Sets the brake mode for the DC motor.
Definition MiniR4DriveDC.h:637
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:290
MiniR4DriveDC::MoveGyro
uint8_t MoveGyro(int16_t power, int16_t Target)
Sets the power level with gyro guidance.
Definition MiniR4DriveDC.h:419
MiniR4DriveDC::setMoveSyncPID
bool setMoveSyncPID(float kp, float ki, float kd)
Sets the PID parameters for DriveDC MoveSync.
Definition MiniR4DriveDC.h:88
MiniR4DriveDC::resetCounter
uint8_t resetCounter(void)
Resets the drivebase encoder counter/degrees to zero.
Definition MiniR4DriveDC.h:617
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:198
MiniR4DriveDC::getDegrees
int32_t getDegrees(void)
Gets the estimated degrees of the drivebase.
Definition MiniR4DriveDC.h:600
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:500
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:245
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:373
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:654
MiniR4DriveDC::setMoveGyroPID
bool setMoveGyroPID(float kp, float ki, float kd)
Sets the PID parameters for DriveDC MoveGyro.
Definition MiniR4DriveDC.h:107
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:548