下半身制御用ライブラリ Odometry...自己位置推定 Mecanum...メカナムホイール用 Bezier...ベジエ曲線 RoboClaw...MD用
Dependents: TOUTEKI_all_mbed mbed_test_program
Revision 3:9c3f2662974e, committed 2018-09-04
- Comitter:
- yuki17100
- Date:
- Tue Sep 04 14:22:13 2018 +0000
- Parent:
- 2:f1085de93385
- Commit message:
- Mecanum????????
Changed in this revision
Mecanum.cpp | Show annotated file Show diff for this revision Revisions of this file |
Mecanum.h | Show annotated file Show diff for this revision Revisions of this file |
diff -r f1085de93385 -r 9c3f2662974e Mecanum.cpp --- a/Mecanum.cpp Sun Sep 02 11:36:38 2018 +0000 +++ b/Mecanum.cpp Tue Sep 04 14:22:13 2018 +0000 @@ -4,17 +4,32 @@ } +void Mecanum::command(double (&duty)[4]){ + MD.SpeedM1(ADR_MD1, (int)(duty[0]*CMD)); + MD.SpeedM2(ADR_MD1, (int)(duty[2]*CMD)); + MD.SpeedM1(ADR_MD2, (int)(duty[3]*CMD)); + MD.SpeedM2(ADR_MD2, (int)(duty[1]*CMD)); +} + void Mecanum::move(double direction, double speed, double revolution){ double duty[4]; - duty[0] = (cos(direction) + sin(direction))*speed - revolution;//右前 - duty[1] = (cos(direction) - sin(direction))*speed + revolution;//左前 - duty[2] = (cos(direction) - sin(direction))*speed - revolution;//右後 - duty[3] = (cos(direction) + sin(direction))*speed + revolution;//左後 + duty[0] = (cos(direction) - sin(direction))*speed - revolution;//右前 + duty[1] = (cos(direction) + sin(direction))*speed + revolution;//左前 + duty[2] = (cos(direction) + sin(direction))*speed - revolution;//右後 + duty[3] = (cos(direction) - sin(direction))*speed + revolution;//左後 + + command(duty); - MD.SpeedM1(ADR_MD1, (int)(duty[0]*CMD));//右前 - MD.SpeedM2(ADR_MD1, (int)(duty[2]*CMD));//右後 - MD.SpeedM1(ADR_MD2, (int)(duty[3]*CMD));//左後 - MD.SpeedM2(ADR_MD2, (int)(duty[1]*CMD));//左前 +} + +void Mecanum::rectang(double x,double y,double rotation){ + double duty[4]; + duty[0] = y - x - rotation; + duty[1] = y + x + rotation; + duty[2] = y + x - rotation; + duty[3] = y - x + rotation; + + command(duty); } \ No newline at end of file
diff -r f1085de93385 -r 9c3f2662974e Mecanum.h --- a/Mecanum.h Sun Sep 02 11:36:38 2018 +0000 +++ b/Mecanum.h Tue Sep 04 14:22:13 2018 +0000 @@ -9,18 +9,21 @@ #define PIN_MD p13 -#define CMD 10 +#define CMD (10000/1000) class Mecanum{ public: Mecanum(); void move(double direction, double speed, double revolution); + void rectang(double x, double y, double rotation); RoboClaw MD; private: + void command(double (&duty)[4]); + }; #endif \ No newline at end of file