Diff: User.cpp

Revision:

21:d9f5b74d5034

Parent:

20:b84beed117ef

Child:

22:e88dd3acec2b

--- a/User.cpp	Wed Aug 23 13:20:31 2017 +0000
+++ b/User.cpp	Wed Aug 23 14:29:18 2017 +0000
@@ -17,9 +17,10 @@
 //オムニホイール
 
 /*　　　正転の向き
-    　　↙　　　↖
-
-　　　　　  →        */
+　　　　ｌ↙　　　↖ｆ　　　　
+　　　　　　　　　　　　　　
+　　　　　 　→
+　　　　　　ｒ　　　　　　 */
 PwmOut motor_f_1(p21);
 PwmOut motor_f_2(p22);
 PwmOut motor_l_1(p23);
@@ -27,8 +28,8 @@
 PwmOut motor_r_1(p25);
 PwmOut motor_r_2(p26);
 
-double fai=1/3*Pi;//φ
-//個体差
+double fai=60;//φ
+//個体差で出力調整
 double power_f=0.8;
 double power_l=0.8;
 double power_r=0.8;
@@ -207,30 +208,30 @@
         if(LSX>=center && LSX<=255 && LSY>=center && LSY<=255) { //第四象限
             sita = -1.0*(atan2((double)LSY-center,(double)LSX-center))*180/Pi;
             sita_2=90-sita;
-            M1=sin((sita_2-fai+0)*Pi/180)*power_f;
-            M2=sin((sita_2-fai+240)*Pi/180)*power_l;
-            M3=sin((sita_2-fai+120)*Pi/180)*power_r;
+            M1=sin((sita_2-(fai+0))*Pi/180)*power_f;
+            M2=sin((sita_2-(fai+240))*Pi/180)*power_l;
+            M3=sin((sita_2-(fai+120))*Pi/180)*power_r;
             motor_act();
         } else if(LSX>=0 && LSX<=center && LSY>=center && LSY<=255) { //第三象限
             sita = -1.0*(atan2((double)LSY-center,(double)LSX-center))*180/Pi;
             sita_2=90-sita;
-            M1=sin((sita_2-fai+0)*Pi/180)*power_f;
-            M2=sin((sita_2-fai+240)*Pi/180)*power_l;
-            M3=sin((sita_2-fai+120)*Pi/180)*power_r;
+            M1=sin((sita_2-(fai+0))*Pi/180)*power_f;
+            M2=sin((sita_2-(fai+240))*Pi/180)*power_l;
+            M3=sin((sita_2-(fai+120))*Pi/180)*power_r;
             motor_act();
         } else if(LSX>=0 && LSX<=center && LSY>=0 && LSY<=center) { //第二象限
             sita = -1.0*(atan2((double)LSY-center,(double)LSX-center))*180/Pi;
             sita_2=90-sita;
-            M1=sin((sita_2-fai+0)*Pi/180)*power_f;
-            M2=sin((sita_2-fai+240)*Pi/180)*power_l;
-            M3=sin((sita_2-fai+120)*Pi/180)*power_r;
+            M1=sin((sita_2-(fai+0))*Pi/180)*power_f;
+            M2=sin((sita_2-(fai+240))*Pi/180)*power_l;
+            M3=sin((sita_2-(fai+120))*Pi/180)*power_r;
             motor_act();
         } else if(LSX>=center && LSX<=255 && LSY>=0 && LSY<=center) { //第一象限
             sita = -1.0*(atan2((double)LSY-center,(double)LSX-center))*180/Pi;
             sita_2=90-sita;
-            M1=sin((sita_2-fai+0)*Pi/180)*power_f;
-            M2=sin((sita_2-fai+240)*Pi/180)*power_l;
-            M3=sin((sita_2-fai+120)*Pi/180)*power_r;
+            M1=sin((sita_2-(fai+0))*Pi/180)*power_f;
+            M2=sin((sita_2-(fai+240))*Pi/180)*power_l;
+            M3=sin((sita_2-(fai+120))*Pi/180)*power_r;
             motor_act();
         }
 //真っすぐだけのプログラム(いらない）