Diff: headerfile_unuse/mecanum_.h

Revision:

66:1664ee92539d

Parent:

56:a7bd860b85b6

diff -r 5e4c2e5494ae -r 1664ee92539d headerfile_unuse/mecanum_.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/headerfile_unuse/mecanum_.h	Sun Sep 24 05:25:03 2017 +0000
@@ -0,0 +1,126 @@
+/*
+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) 
+
+.v1 //(返り値[float] -1.0 ~ 1.0) モータ1用出力
+.v2 //(返り値[float] -1.0 ~ 1.0) モータ2用出力
+.v3 //(返り値[float] -1.0 ~ 1.0) モータ3用出力
+.v4 //(返り値[float] -1.0 ~ 1.0) モータ4用出力
+
+メカナム設置図
+    ↑→   →↓
+     O---O
+     O---O
+    ←↑   ↓←
+
+
+*/
+
+#define rotaPower 0.6
+#define rotaStickPower 0.35
+#define boost_A 0.5
+#define boost_K 0.9
+#define PI 3.141592
+
+class Mecanum{
+    public:
+        void setupdeg(float Deg){
+                first_deg = Deg;
+        }
+    
+        void sbdbt_cal(float Vx, float Vy, int r1, int l1,float w, float angle){
+            rad = (angle - first_deg)/180*PI;
+            
+            vx = Vx / 2 * cos(rad) - Vy / 2 * sin(rad);
+            vy = Vx / 2 * sin(rad) + Vy / 2 * cos(rad);
+
+            V1 =   vx + vy + rotaPower * (l1 - r1) + rotaStickPower * w;
+            V2 =  -vx - vy + rotaPower * (l1 - r1) + rotaStickPower * w;
+            V3 =  -vx + vy + rotaPower * (l1 - r1) + rotaStickPower * w;
+            V4 =   vx - vy + rotaPower * (l1 - r1) + rotaStickPower * w;
+            
+        }
+        
+        void boost(){
+            if(vy > 0.4){
+                V1 +=  boost_A;
+                V2 += -boost_A;
+                V3 +=  boost_A;
+                V4 += -boost_A;
+            }else if(vy < -0.4){
+                V1 += -boost_A;
+                V2 +=  boost_A;
+                V3 += -boost_A;
+                V4 +=  boost_A;
+            }
+        }
+        
+        void boost_forward(){
+                V1 = -boost_K;
+                V2 =  boost_K;
+                V3 = -boost_K;
+                V4 =  boost_K;
+        }
+        
+        void boost_back(){
+                V1 =  boost_K;
+                V2 = -boost_K;
+                V3 =  boost_K;
+                V4 = -boost_K;
+        }
+        
+        void boost_right(){
+                V1 =  boost_K;
+                V2 = -boost_K;
+                V3 = -boost_K;
+                V4 =  boost_K;
+        }
+        
+        void boost_left(){
+                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;
+            V3 = -Vx / 2 + Vy / 2;
+            V4 = -Vx / 2 - Vy / 2;
+        }
+        
+        float v1(){
+            return V1; 
+        } 
+        
+        float v2(){
+            return V2;  
+        } 
+        
+        float v3(){
+            return V3;  
+        } 
+        
+        float v4(){
+            return V4;
+        }
+        
+        float VX(){
+            return vx;
+        } 
+        
+        float VY(){
+            return vy;
+        } 
+        
+        
+    private:
+        float V1, V2, V3, V4, vx, vy;
+        float first_deg, rad;
+};
\ No newline at end of file