Diff: mecanum.h

Revision:

0:bf96e953cdb8

Child:

5:6efda58ff71b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mecanum.h	Mon Jun 26 09:59:14 2017 +0000
@@ -0,0 +1,85 @@
+/*
+Mecanum mecanum
+
+.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.3
+#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) + w;
+            V2 =   vx - vy + rotaPower * (l1 - r1) + w;
+            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 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