Diff: User.cpp

Revision:

20:b84beed117ef

Parent:

19:a3f57c9833b6

Child:

21:d9f5b74d5034

--- a/User.cpp	Wed Aug 23 10:10:26 2017 +0000
+++ b/User.cpp	Wed Aug 23 13:20:31 2017 +0000
@@ -17,9 +17,9 @@
 //オムニホイール
 
 /*　　　正転の向き
-    　　　　←
+    　　↙　　　↖
 
-　　　　　↘    ↗      */
+　　　　　  →        */
 PwmOut motor_f_1(p21);
 PwmOut motor_f_2(p22);
 PwmOut motor_l_1(p23);
@@ -29,13 +29,24 @@
 
 double fai=1/3*Pi;//φ
 //個体差
-double power_f=1;
-double power_l=1;
-double power_r=1;
+double power_f=0.8;
+double power_l=0.8;
+double power_r=0.8;
 
 double M1;
 double M2;
 double M3;
+
+//ジョイスティックの中心座標
+double center=127;
+
+
+//ジョイスティック閾値
+double delta=90;
+double bound_p=center+delta;
+double bound_m=center-delta;
+
+
 //回転の比
 double roll_spd=0.5;
 void motor_act()
@@ -110,62 +121,62 @@
     //ここより下にプログラムを書く
     //spi通信用プログラム
 
-    int a,b,c,d,e,f,g,h;
+    int L_up,L_down,L_open,L_close,R_up,R_down,R_open,R_close;
     int send = 0;
 
 
 
     if((ButtonState >> BUTTONUP)&1 == 1) {//対応するボタンを書く(今回上ボタン
-        a = 1;
+        L_up = 1;
     } else {
-        a = 0;
+        L_up = 0;
     }
 
     if((ButtonState >> BUTTONDOWN)&1 == 1) {//対応するボタンを書く(今回下ボタン
-        b = 2;
+        L_down = 2;
     } else {
-        b = 0;
+        L_down = 0;
     }
 
     if((ButtonState >> BUTTONL1)&1 == 1) {//対応するボタンを書く(今回L1ボタン
-        c = 4;
+        L_open = 4;
     } else {
-        c = 0;
+        L_open = 0;
     }
 
     if((ButtonState >> BUTTONL2)&1 == 1) {//対応するボタンを書く(今回L2ボタン
-        d = 8;
+        L_close = 8;
     } else {
-        d = 0;
+        L_close = 0;
     }
 
     if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {//対応するボタンを書く(今回△ボタン
-        e = 16;
+        R_up = 16;
     } else {
-        e = 0;
+        R_up = 0;
     }
 
     if((ButtonState >> BUTTONCROSS)&1 == 1) {//対応するボタンを書く(今回×ボタン
-        f = 32;
+        R_down = 32;
     } else {
-        f = 0;
+        R_down = 0;
     }
 
     if((ButtonState >> BUTTONR1)&1 == 1) {//対応するボタンを書く(今回R1ボタン
-        g = 64;
+        R_open = 64;
     } else {
-        g = 0;
+        R_open = 0;
     }
 
     if((ButtonState >> BUTTONR2)&1 == 1) {//対応するボタンを書く(今回R2ボタン
-        h = 128;
+        R_close = 128;
     } else {
-        h = 0;
+        R_close = 0;
     }
 
 
 
-    send = a+b+c+d+e+f+g+h;
+    send = L_up+L_down+L_open+L_close+R_up+R_down+R_open+R_close;
 
     cs = 0;
     spi. write(send);
@@ -175,15 +186,15 @@
 
 
 //オムニホイールのプログラム
-    if(LSX>=110 && LSX<=145 && LSY>=110 && LSY<=145) {
+    if(LSX>=bound_m && LSX<=bound_p && LSY>=bound_m && LSY<=bound_p) {
         M1=0;
         M2=0;
         M3=0;
-        if(RSX>=180 && RSX<=255) {
+        if(RSX>=bound_p && RSX<=255) {
             M1=power_f*roll_spd;
             M2=power_l*roll_spd;
             M3=power_r*roll_spd;
-        } else if(RSX>=0 && RSX<=80) {
+        } else if(RSX>=0 && RSX<=bound_m) {
             M1=-1.0*power_f*roll_spd;
             M2=-1.0*power_l*roll_spd;
             M3=-1.0*power_r*roll_spd;
@@ -193,37 +204,37 @@
 
         sita=0;
     } else {
-
-        if(LSX>=129 && LSX<=255 && LSY>=129 && LSY<=255) { //第四象限
-            sita = -1.0*(atan2((double)LSY-128,(double)LSX-128))*180/Pi;
+        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)*Pi/180)*power_f;
+            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<=127 && LSY>=129 && LSY<=255) { //第三象限
-            sita = -1.0*(atan2((double)LSY-128,(double)LSX-128))*180/Pi;
+        } 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)*Pi/180)*power_f;
+            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<=127 && LSY>=0 && LSY<=127) { //第二象限
-            sita = -1.0*(atan2((double)LSY-128,(double)LSX-128))*180/Pi;
+        } 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)*Pi/180)*power_f;
+            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>=129 && LSX<=255 && LSY>=0 && LSY<=127) { //第一象限
-            sita = -1.0*(atan2((double)LSY-128,(double)LSX-128))*180/Pi;
+        } 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)*Pi/180)*power_f;
+            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==255) {
                  sita = 0;
                  sita_2=90-sita;
@@ -262,7 +273,7 @@
 
 
 
-    printf("motor_f_1:%f\motor_l_1:%f\motor_r_1:%f\sita:%f\r\n",M1,M2,M3,sita);
+    printf("motor_f_1:%.4f\t\motor_l_1:%.4f\t\motor_r_1:%.4f\t\sita:%f\r\n",M1,M2,M3,sita);