yuto kawamura / Mbed 2 deprecated F3RC_syudou_master

主導機　mbed用のプログラムです

Dependencies:   mbed

Fork of F3RC_syudou_master by F3RC1班

Diff: User.cpp

Revision:
19:a3f57c9833b6
Parent:
18:2579c275ef57
--- a/User.cpp	Wed Aug 23 03:31:34 2017 +0000
+++ b/User.cpp	Wed Aug 23 10:10:26 2017 +0000
@@ -28,8 +28,7 @@
 PwmOut motor_r_2(p26);
 
 double fai=1/3*Pi;//φ
-
-
+//個体差
 double power_f=1;
 double power_l=1;
 double power_r=1;
@@ -37,7 +36,8 @@
 double M1;
 double M2;
 double M3;
-
+//回転の比
+double roll_spd=0.5;
 void motor_act()
 {
     if(M1 >=0) {
@@ -170,80 +170,98 @@
     cs = 0;
     spi. write(send);
     cs = 1;
-    printf("%d\r\n",send);
+    // printf("%d\r\n",send);
+
 
 
-    if(LSX>=150 && LSX<=255 && LSY>=150 && LSY<=255) { //第一象限
-        sita = -1.0*(atan2((double)LSY-128,(double)LSX-128))*180/Pi;
-        sita_2=90-sita;
-        M1=sin((sita_2-fai)*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<=80 && LSY>=150 && LSY<=255) { //第二象限
-        sita = -1.0*(atan2((double)LSY-128,(double)LSX-128))*180/Pi;
-        sita_2=90-sita;
-        M1=sin((sita_2-fai)*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<=80 && LSY>=0 && LSY<=80) { //第三象限
-        sita = -1.0*(atan2((double)LSY-128,(double)LSX-128))*180/Pi;
-        sita_2=90-sita;
-        M1=sin((sita_2-fai)*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>=150 && LSX<=255 && LSY>=0 && LSY<=80) { //第四象限
-        sita = -1.0*(atan2((double)LSY-128,(double)LSX-128))*180/Pi;
-        sita_2=90-sita;
-        M1=sin((sita_2-fai)*Pi/180)*power_f;
-        M2=sin((sita_2-fai+240)*Pi/180)*power_l;
-        M3=sin((sita_2-fai+120)*Pi/180)*power_r;
+//オムニホイールのプログラム
+    if(LSX>=110 && LSX<=145 && LSY>=110 && LSY<=145) {
+        M1=0;
+        M2=0;
+        M3=0;
+        if(RSX>=180 && RSX<=255) {
+            M1=power_f*roll_spd;
+            M2=power_l*roll_spd;
+            M3=power_r*roll_spd;
+        } else if(RSX>=0 && RSX<=80) {
+            M1=-1.0*power_f*roll_spd;
+            M2=-1.0*power_l*roll_spd;
+            M3=-1.0*power_r*roll_spd;
+        }
+
         motor_act();
 
-    } else if(LSX==255) {
-        sita = 0;
-        sita_2=90-sita;
-        M1=sin((sita_2-fai)*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(LSY==255) {
-        sita = 90;
-        sita_2=90-sita;
-        M1=sin((sita_2-fai)*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();
+        sita=0;
+    } else {
 
-    } else if(LSX==0) {
-        sita = 180;
-        sita_2=90-sita;
-        M1=sin((sita_2-fai)*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();
+        if(LSX>=129 && LSX<=255 && LSY>=129 && LSY<=255) { //第四象限
+            sita = -1.0*(atan2((double)LSY-128,(double)LSX-128))*180/Pi;
+            sita_2=90-sita;
+            M1=sin((sita_2-fai)*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;
+            sita_2=90-sita;
+            M1=sin((sita_2-fai)*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;
+            sita_2=90-sita;
+            M1=sin((sita_2-fai)*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;
+            sita_2=90-sita;
+            M1=sin((sita_2-fai)*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;
+                 M1=sin((sita_2-fai)*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(LSY==0) {
-        sita = 270;
-        sita_2=90-sita;
-        M1=sin((sita_2-fai)*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 {
-        motor_f_1=0;
-        motor_f_2=0;
-        motor_l_1=0;
-        motor_l_2=0;
-        motor_r_1=0;
-        motor_r_2=0;
+             } else if(LSY==255) {
+                 sita = -90;
+                 sita_2=90-sita;
+                 M1=sin((sita_2-fai)*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) {
+                 sita = 180;
+                 sita_2=90-sita;
+                 M1=sin((sita_2-fai)*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(LSY==0) {
+                 sita = 90;
+                 sita_2=90-sita;
+                 M1=sin((sita_2-fai)*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();
+             } */
 
     }
 
 
+
+
     printf("motor_f_1:%f\motor_l_1:%f\motor_r_1:%f\sita:%f\r\n",M1,M2,M3,sita);