yuki honma
下半身制御用ライブラリ Odometry...自己位置推定 Mecanum...メカナムホイール用 Bezier...ベジエ曲線 RoboClaw...MD用
Dependents: TOUTEKI_all_mbed mbed_test_program
Diff: Mecanum.cpp
- Revision:
- 0:62707e16531a
- Child:
- 1:698586aa0c5a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Mecanum.cpp Sat Sep 01 09:07:35 2018 +0000
@@ -0,0 +1,20 @@
+#include "Mecanum.h"
+
+Mecanum::Mecanum() : MD(115200, PIN_MD, NC){
+}
+
+
+void Mecanum::move(double direction, double speed, double revolution){
+ double duty[4];
+
+ duty[0] = (sin(direction) - cos(direction))*speed - revolution;//右前
+ duty[1] = (sin(direction) + cos(direction))*speed + revolution;//左前
+ duty[2] = (sin(direction) - cos(direction))*speed + revolution;//右後
+ duty[3] = (sin(direction) + cos(direction))*speed - revolution;//左後
+
+ 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));//左前
+
+}
\ No newline at end of file