Diff: main.cpp

Revision:

3:6633e752a5de

Parent:

2:2b635b7e0bb6

--- a/main.cpp	Wed Jun 26 07:44:09 2019 +0000
+++ b/main.cpp	Tue Jul 02 09:31:00 2019 +0000
@@ -34,7 +34,7 @@
     int b[11];//自作コントローラー関連
     double stickRad[2],stickNorn[2],raded[2],Norned[2];
 
-    bool b7 = 1,b9 = 1;//b[7]とb[9]のボタン判断
+    bool b8 = 1,b10 = 1;//b[8]とb[10]のボタン判断
 
     double acc[4] = {0,0,0,0},angle,control,con,func,gain = 0;//accはアクセラレート：加速の略　angleはジャイロセンサの値を入れる  control,conは角度調整に必要
     double idealAngle = 180,puls = 0.1;//funcは台形制御に必要 gainは台形制御の計算に使う idealAngleは角度セットに使う
@@ -53,10 +53,10 @@
     omni.setErrorRange(2);//誤差範囲を設定 +-入力値分の誤差が出る ロボットが止まらない時は大きめに設定すること
 
     omni.limitPower(0.5);//回転速度制限　ここで入力した速さよりも回転は速くならない 正の実数で１より大きくしないこと 必須
-
     while(1)
     {
-        if(R1370.update() == 0 && pad.receiveState() == 1)//ジャイロセンサが動いてかつコントローラーと通信していれば
+        R1370.update();
+        if(/*R1370.update() != 1 && */pad.receiveState() != 0)//ジャイロセンサが動いてかつコントローラーと通信していれば
         {        
             angle = R1370.getAngle();//ジャイロの出した角度を保存 必須
 
@@ -67,25 +67,25 @@
                 stickRad[i] = PI - pad.getRadian(i);
                 stickNorn[i] = pad.getNorm(i);
             }
-            pc.printf("%d %d\n\r",b[7],b[9]);
+            pc.printf("%d %d\n\r",b[8],b[10]);
 /*一度ボタンを押すとその方向に90度回転*/            
-            if(b[7] != b7)
+            if(b[8] != b8)
             {
-                if(b7 == 1)
+                if(b8 == 1)
                 {
                     idealAngle -= 90;
                     gain = 0;
                 }
-                b7 = b[7];
+                b8 = b[8];
             }
-            if(b[9] != b9)
+            if(b[10] != b10)
             {
-                if(b9 == 1)
+                if(b10 == 1)
                 {
                     idealAngle += 90;
                     gain = 0;
                 }
-                b9 = b[9];
+                b10 = b[10];
             }
             if(idealAngle > 360)
             {
@@ -107,17 +107,15 @@
 この二つはどちらか一方のみを使うこと
 どっちも使ってみてから決めてほしい*/
 
-            if(fabs(raded[0] - stickRad[0]) > 2 || fabs(raded[1] - stickRad[1]) > 2 || fabs(Norned[0] - stickNorn[0]) > 0.2 || fabs(Norned[1] - stickNorn[1] > 0.2) )
+            if(fabs(raded[0] - stickRad[0]) > 2 || fabs(Norned[0] - stickNorn[0]) > 0.2)
             {
-                gain = 0;//もしスティックが大きく動いたらgain = 0
+                gain -= 0;//もしスティックが大きく動いたらgain = 0
             }
 
             func = omni.lFunc(0.25,gain);//一次関数の値を取り出す omni.lFunc(比例定数,gain)
 
             raded[0] = stickRad[0];  //値比較用
-            raded[1] = stickRad[1];  //
             Norned[0] = stickNorn[0];//
-            Norned[1] = stickNorn[1];//
 
 /* スティックが傾いていないまたはどちらも傾いていると平行移動しない*/            
             if( ((stickNorn[0] == 0) && (stickNorn[1] == 0)) || ((stickNorn[0] != 0) && (stickNorn[1] != 0)) )
@@ -127,50 +125,40 @@
                     acc[i] = (omni.hMove(10,i) + con) * func;
                 }//+controlまたは +conで角度がずれたら修正 この時は比例制御を使った//omni.hMove(角度radian,タイヤ番号)　で速度を返す  角度が10なら平行移動しない  必須
                 gain += puls;//どんどんモーターの値が大きくなる
-
             }
             else if(stickNorn[0] != 0 && stickNorn[1] == 0) //stick[0]だけを動かしているとき
             {
                 for(int i = 0; i < 4; i++)
                 {
-                    acc[i] = (omni.hMove(stickRad[0], i) * stickNorn[0] + con) * func;  //方向がロボットの向きによって変化する
+                    acc[i] = (omni.hMove(stickRad[0] + (angle * (PI / 180)), i) * stickNorn[0] + con) * func;//ロボットの向きに左右されずに動く
                 }
                 gain += puls;//どんどん（ry
             }
 
-            else //stick[1]だけを動かしているとき
-            {
-                for(int i = 0; i < 4; i++)
-                {
-                    acc[i] = (omni.hMove(stickRad[1] + (angle * (PI / 180)), i) * stickNorn[1] + con) * func;//ロボットの向きに左右されずに動く
-                }
-             gain += puls;//どんどん（ry
-            }
-
             for(int i = 0; i < 4;i++) //モーターに出力 必須
             {
-                if(acc[i] > 0.9)
+                if(acc[i] > 0.5)
                 {
-                    acc[i] = 0.9;
+                    acc[i] = 0.5;
                 }
-                else if(acc[i] < -0.9)
+                else if(acc[i] < -0.5)
                 {
-                    acc[i] = -0.9;
+                    acc[i] = -0.5;
                 }
                 ikarashi[i].setSpeed(acc[i]);
                 //pc.printf("|%f| ",acc[i]);
             }
-            pc.printf("|%f| |%f|",acc[0],idealAngle);
-        }
+            pc.printf("|%.2f| ",acc[0]);
 
-        else//ジャイロ,コントローラーが動いていなかったら
+        }
+        if(/*R1370.update() != 0 || */pad.receiveState() == 0)//ジャイロ,コントローラーが動いていなかったら
         {
-            pc.printf("error\n\r");   //errorを表示して動かない
+            pc.printf("e\n\r");   //errorを表示して動かない
 
             for(int i = 0; i < 4;i++)
             {
                 ikarashi[i].setSpeed(0);//モーターに0 必須
             }
-        }                         
+        }                    
     }
 }