Diff: mecanum.h

Revision:

5:6efda58ff71b

Parent:

0:bf96e953cdb8

Child:

10:04f2a82cfd89

diff -r a6cc2f03e69b -r 6efda58ff71b mecanum.h
--- a/mecanum.h	Fri Jun 30 01:18:54 2017 +0000
+++ b/mecanum.h	Fri Jun 30 05:40:02 2017 +0000
@@ -1,6 +1,7 @@
 /*
 Mecanum mecanum
 
+Sbdbtの入力 +y↑ -y↓ +x→ -x←
 .setupdeg([float]Deg)   //角度センサの初期値設定
 .sbdbt_cal([float]Vx, [float]Vy, [int]con_rl, [int]con_l1, [float]w, [float]angle) //使わない部分は0を代入
 .xy_cal([float]Vx, [float]Vy) //xyの代入値(-1.0 ~ 1.0) 
@@ -11,15 +12,16 @@
 .v4 //(返り値[float] -1.0 ~ 1.0) モータ4用出力
 
 メカナム設置図
-    ↑→   ←↑
+    ↑→   →↓
      O---O
      O---O
-    ←↑   ↑→
+    ←↑   ↓←
 
 
 */
 
 #define rotaPower 0.3
+#define boost_K 0.5
 #define PI 3.141592
 
 class Mecanum{
@@ -39,14 +41,22 @@
             V3 =   vx + vy + rotaPower * (l1 - r1) + w;
             V4 =  -vx - vy + rotaPower * (l1 - r1) + w;
             
-            /*
-            V1 =   vx + vy + rotaPower * (r1 - l1) + w;
-            V2 =   vx - vy + rotaPower * (r1 - l1) + w;
-            V3 =  -vx + vy + rotaPower * (r1 - l1) + w;
-            V4 =  -vx - vy + rotaPower * (r1 - l1) + w;
-            */
         }
         
+        void boost(){
+            if(vy > 0.4){
+                V1 +=  boost_K;
+                V2 += -boost_K;
+                V3 +=  boost_K;
+                V4 += -boost_K;
+            }else if(vy < -0.4){
+                V1 += -boost_K;
+                V2 +=  boost_K;
+                V3 += -boost_K;
+                V4 +=  boost_K;
+            }
+        }        
+        
         void xy_cal(float Vx, float Vy){
             V1 =  Vx / 2 + Vy / 2;
             V2 =  Vx / 2 - Vy / 2;