yuki honma / UnderBody

下半身制御用ライブラリ Odometry...自己位置推定 Mecanum...メカナムホイール用 Bezier...ベジエ曲線 RoboClaw...MD用

Dependents:   TOUTEKI_all_mbed mbed_test_program

Files at this revision

API Documentation at this revision

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