下半身制御用ライブラリ Odometry...自己位置推定 Mecanum...メカナムホイール用 Bezier...ベジエ曲線 RoboClaw...MD用
Dependents: TOUTEKI_all_mbed mbed_test_program
Mecanum.cpp@3:9c3f2662974e, 2018-09-04 (annotated)
- Committer:
- yuki17100
- Date:
- Tue Sep 04 14:22:13 2018 +0000
- Revision:
- 3:9c3f2662974e
- Parent:
- 2:f1085de93385
Mecanum????????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yuki17100 | 0:62707e16531a | 1 | #include "Mecanum.h" |
yuki17100 | 0:62707e16531a | 2 | |
yuki17100 | 0:62707e16531a | 3 | Mecanum::Mecanum() : MD(115200, PIN_MD, NC){ |
yuki17100 | 0:62707e16531a | 4 | } |
yuki17100 | 0:62707e16531a | 5 | |
yuki17100 | 0:62707e16531a | 6 | |
yuki17100 | 3:9c3f2662974e | 7 | void Mecanum::command(double (&duty)[4]){ |
yuki17100 | 3:9c3f2662974e | 8 | MD.SpeedM1(ADR_MD1, (int)(duty[0]*CMD)); |
yuki17100 | 3:9c3f2662974e | 9 | MD.SpeedM2(ADR_MD1, (int)(duty[2]*CMD)); |
yuki17100 | 3:9c3f2662974e | 10 | MD.SpeedM1(ADR_MD2, (int)(duty[3]*CMD)); |
yuki17100 | 3:9c3f2662974e | 11 | MD.SpeedM2(ADR_MD2, (int)(duty[1]*CMD)); |
yuki17100 | 3:9c3f2662974e | 12 | } |
yuki17100 | 3:9c3f2662974e | 13 | |
yuki17100 | 0:62707e16531a | 14 | void Mecanum::move(double direction, double speed, double revolution){ |
yuki17100 | 0:62707e16531a | 15 | double duty[4]; |
yuki17100 | 0:62707e16531a | 16 | |
yuki17100 | 3:9c3f2662974e | 17 | duty[0] = (cos(direction) - sin(direction))*speed - revolution;//右前 |
yuki17100 | 3:9c3f2662974e | 18 | duty[1] = (cos(direction) + sin(direction))*speed + revolution;//左前 |
yuki17100 | 3:9c3f2662974e | 19 | duty[2] = (cos(direction) + sin(direction))*speed - revolution;//右後 |
yuki17100 | 3:9c3f2662974e | 20 | duty[3] = (cos(direction) - sin(direction))*speed + revolution;//左後 |
yuki17100 | 3:9c3f2662974e | 21 | |
yuki17100 | 3:9c3f2662974e | 22 | command(duty); |
yuki17100 | 0:62707e16531a | 23 | |
yuki17100 | 3:9c3f2662974e | 24 | } |
yuki17100 | 3:9c3f2662974e | 25 | |
yuki17100 | 3:9c3f2662974e | 26 | void Mecanum::rectang(double x,double y,double rotation){ |
yuki17100 | 3:9c3f2662974e | 27 | double duty[4]; |
yuki17100 | 0:62707e16531a | 28 | |
yuki17100 | 3:9c3f2662974e | 29 | duty[0] = y - x - rotation; |
yuki17100 | 3:9c3f2662974e | 30 | duty[1] = y + x + rotation; |
yuki17100 | 3:9c3f2662974e | 31 | duty[2] = y + x - rotation; |
yuki17100 | 3:9c3f2662974e | 32 | duty[3] = y - x + rotation; |
yuki17100 | 3:9c3f2662974e | 33 | |
yuki17100 | 3:9c3f2662974e | 34 | command(duty); |
yuki17100 | 0:62707e16531a | 35 | } |