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 
--- 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

--- 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