主導機 mbed用のプログラムです 改良しました

Dependencies:   mbed

Fork of F3RC_syudou_master_3 by 日記

Files at this revision

API Documentation at this revision

Comitter:
yuto17320508
Date:
Thu Sep 14 01:20:12 2017 +0000
Parent:
25:d5588a50f069
Commit message:
a

Changed in this revision

User.cpp Show annotated file Show diff for this revision Revisions of this file
--- a/User.cpp	Fri Aug 25 12:23:18 2017 +0000
+++ b/User.cpp	Thu Sep 14 01:20:12 2017 +0000
@@ -31,29 +31,35 @@
 
 double fai=60;//φ
 //個体差で出力調整
-double power_f=0.2;
-double power_l=0.2;
-double power_r=0.2;
+double power_f=0.32;
+double power_l=0.32;
+double power_r=0.32;
 
 double M1;
 double M2;
 double M3;
 
+double accel=1.3;
 //ジョイスティックの中心座標
 double center=127;
 
-
 //ジョイスティック閾値
 double delta=90;
 double bound_p=center+delta;
 double bound_m=center-delta;
 
+//回転の比
+ //移動中の回転
+double roll_spd=0.4;
+ //横移動中の回転
+double roll_spd2=0.1;
+ //横移動のスピード
+double yoko_spd=0.9;
 
-//回転の比
-double roll_spd=0.5;
 //モーターの動作
 void motor_act()
 {
+    //絶対値をつける関数
     if(M1 >=0) {
         motor_f_1=M1;
         motor_f_2=0;
@@ -75,24 +81,20 @@
         motor_r_1=0;
         motor_r_2=-M3;
     }
-
 }
 //ジョイスティック入力値の偏角
 double sita;
 //関数代入用の角度調整
 double sita_2;
 
-
 void UserLoopSetting()
 {
-
     motor_f_1.period_us(100);
     motor_f_2.period_us(100);
     motor_l_1.period_us(100);
     motor_l_2.period_us(100);
     motor_r_1.period_us(100);
     motor_r_2.period_us(100);
-
 }
 
 void UserLoop(char n,const u8* data)
@@ -118,41 +120,38 @@
         ButtonState =  ((ps3report*)(data + 1))->ButtonState;
     }
     //ここより下にプログラムを書く
-    //spi通信用プログラム
-
+    //BUS用プログラム
     int flag=0;
     num=0;
-
-
-    if((ButtonState >> BUTTONUP)&1 == 1) {//対応するボタンを書く(今回上ボタン
+    if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {//対応するボタンを書く
         num = 1;
         flag=flag+1;
     }
-    if((ButtonState >> BUTTONDOWN)&1 == 1) { //対応するボタンを書く(今回下ボタン
+    if((ButtonState >> BUTTONCROSS)&1 == 1) { //対応するボタンを書く
         num = 2;
         flag=flag+1;
     }
-    if((ButtonState >> BUTTONL1)&1 == 1) { //対応するボタンを書く(今回L1ボタン
+    if((ButtonState >> BUTTONR1)&1 == 1) { //対応するボタンを書く
         num = 3;
         flag=flag+1;
     }
-    if((ButtonState >> BUTTONL2)&1 == 1) { //対応するボタンを書く(今回L2ボタン
+    if((ButtonState >> BUTTONR2)&1 == 1) { //対応するボタンを書く
         num = 4;
         flag=flag+1;
     }
-    if((ButtonState >> BUTTONTRIANGEL)&1 == 1) { //対応するボタンを書く(今回△ボタン
+    if((ButtonState >> BUTTONUP)&1 == 1) { //対応するボタンを書く
         num = 5;
         flag=flag+1;
     }
-    if((ButtonState >> BUTTONCROSS)&1 == 1) { //対応するボタンを書く(今回×ボタン
+    if((ButtonState >> BUTTONDOWN)&1 == 1) { //対応するボタンを書く
         num = 6;
         flag=flag+1;
     }
-    if((ButtonState >> BUTTONR1)&1 == 1) { //対応するボタンを書く(今回R1ボタン
+    if((ButtonState >> BUTTONL1)&1 == 1) { //対応するボタンを書く
         num = 7;
         flag=flag+1;
     }
-    if((ButtonState >> BUTTONR2)&1 == 1) { //対応するボタンを書く(今回R2ボタン
+    if((ButtonState >> BUTTONL2)&1 == 1) { //対応するボタンを書く
         num = 8;
         flag=flag+1;
     }
@@ -162,73 +161,79 @@
     out=num;
     printf("%d\r\n",num);
     //オムニホイールのプログラム
+
+    if((ButtonState >> BUTTONRANALOG)&1 == 1) {//押し込みで速度変化
+        roll_spd=0.8;
+    } else {
+        roll_spd=0.4;
+    }
     if(LSX>=bound_m && LSX<=bound_p && LSY>=bound_m && LSY<=bound_p) {
         M1=0;
         M2=0;
         M3=0;
-        if(RSX>=bound_p && 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<=bound_m) {
+        } 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;
         }
-
-        motor_act();
+        sita=0;
 
-        sita=0;
-    } else {
+    } else {//左スティック全方向
         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;
-        motor_act();
+        if(RSX>=bound_p && RSX<=255) {//進行中の右スティック右
+            M1=M1+power_f*roll_spd;
+            M2=M2+power_l*roll_spd;
+            M3=M3+power_r*roll_spd;
+        } else if(RSX>=0 && RSX<=bound_m) {//進行中の右スティック左
+            M1=M1+-1.0*power_f*roll_spd;
+            M2=M2+-1.0*power_l*roll_spd;
+            M3=M3+-1.0*power_r*roll_spd;
+        }
     }
-//真っすぐだけのプログラム(いらない)
-
-    /*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();
-
-         } 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();
-         } */
+    //真横移動
+    if((ButtonState >> BUTTONLEFT)&1 == 1) { //十字キー左
+        sita = 180;
+        sita_2=90-sita;
+        M1=sin((sita_2-(fai+0))*Pi/180)*power_f*yoko_spd;
+        M2=sin((sita_2-(fai+240))*Pi/180)*power_l*yoko_spd;
+        M3=sin((sita_2-(fai+120))*Pi/180)*power_r*yoko_spd;
+        if(RSX>=bound_p && RSX<=255) {//横移動中の右スティック右
+            M1=M1+power_f*roll_spd2;
+            M2=M2+power_l*roll_spd2;
+            M3=M3+power_r*roll_spd2;
+        } else if(RSX>=0 && RSX<=bound_m) {//横移動中の右スティック左
+            M1=M1+-1.0*power_f*roll_spd2;
+            M2=M2+-1.0*power_l*roll_spd2;
+            M3=M3+-1.0*power_r*roll_spd2;
+        }
+    }
+    if((ButtonState >> BUTTONRIGHT)&1 == 1) { //十字キー右
+        sita = 0;
+        sita_2=90-sita;
+        M1=sin((sita_2-(fai+0))*Pi/180)*power_f*yoko_spd;
+        M2=sin((sita_2-(fai+240))*Pi/180)*power_l*yoko_spd;
+        M3=sin((sita_2-(fai+120))*Pi/180)*power_r*yoko_spd;
+        if(RSX>=bound_p && RSX<=255) {//横移動中の右スティック右
+            M1=M1+power_f*roll_spd2;
+            M2=M2+power_l*roll_spd2;
+            M3=M3+power_r*roll_spd2;
+        } else if(RSX>=0 && RSX<=bound_m) {//横移動中の右スティック左
+            M1=M1+-1.0*power_f*roll_spd2;
+            M2=M2+-1.0*power_l*roll_spd2;
+            M3=M3+-1.0*power_r*roll_spd2;
+        }
+    }
+    motor_act();
 
 
-
-
-
+    printf("motor_f:%.4f\t\motor_l:%.4f\t\motor_r:%.4f\t\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);
-
-
-
-}
\ No newline at end of file
+}