Diff: main.cpp

Revision:

0:09ef6f968eef

Child:

1:c26a135080b1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Jun 10 14:25:27 2019 +0000
@@ -0,0 +1,102 @@
+/*
+ *  //　が縦に連続して並んでいたらそこはひとまとまりになっていると考えて
+ *  
+ *  変数名は変えていいし、タイマー関連で テストで使った と書いてあるものは必要ない
+ *
+ *  何かわからないことがあったらその都度聞いて
+ */
+
+#include "mbed.h"                               //     
+#include "gyro.h"                               //
+#include "ikarashiMDC.h"                        //
+#include "R1370.h"                              //  必須
+Serial pc(USBTX, USBRX, 115200);                //
+Serial serial(PC_6,PC_7);                       //
+DigitalOut serialcontrol(D2);                   //
+R1370 R1370(PB_6,PA_10);                        //
+
+gyro omni(4); //オムニホイールの数を入力
+
+Timer rad,spin;   //これは試すときに使ったやつ
+
+ikarashiMDC ikarashi[] {                        //
+    ikarashiMDC(&serialcontrol,2,0,SM,&serial), //
+    ikarashiMDC(&serialcontrol,2,1,SM,&serial), //
+    ikarashiMDC(&serialcontrol,2,2,SM,&serial), //ロボットの使うMDCに応じて変える事
+    ikarashiMDC(&serialcontrol,2,3,SM,&serial)  //
+};                                              //
+
+int main()
+{
+    double acc[4] = {0,0,0,0},angle,control;  //accはアクセラレート：加速の略　angleはジャイロセンサの値を入れる  controlは角度調整に必要
+    
+    serial.baud(115200);          //
+    ikarashi[0].braking = true;   //必須
+    
+    for(int i = 0; i < 4; i++)            //タイヤ角を設定  必須
+    {                                     //omni.setRad(タイヤの番号,タイヤのついてる角度radian値)
+        omni.setRad(i, PI * (i*2+1) / 4); //このプログラムでは4輪の設定をしている
+    }                                     //もし4輪ならこのままでいい
+    
+    rad.start();   //テストするときに使ったやつ
+    spin.start();  //
+    
+    omni.setIdeal(0); //目的の角度を0度に設定   必須
+    while(1)
+    {
+        if(R1370.update() == 0){       //ジャイロセンサが動いていれば
+            angle = R1370.getAngle();  //ジャイロの出した角度を保存 必須
+            
+            if(spin < 5)        //ここから
+            {
+                omni.setIdeal(0); //目的の角度を0度に設定
+            }
+            else if(spin < 10)
+            {
+                omni.setIdeal(90); //90度に設定
+            }
+            else if (spin < 15)
+            {
+                omni.setIdeal(180);//180度に設定
+            }
+            else if (spin < 20)
+            {
+                omni.setIdeal(-90); //-90度に設定　設定範囲は180 ~ -179
+            }
+            else if (spin < 25)
+            {
+                omni.setIdeal(45); //45度に設定
+            }
+            else
+            {
+                spin.reset();
+            }                   //ここまでテストで使ったやつ  条件式は不要だが角度設定で必要
+            
+            control = omni.cAngle(angle); //ジャイロの角度を渡す　調整用の速度をもらう  必須
+            
+            if(rad > (PI * 2 + 1)) //
+            {                      //
+                rad.reset();       //テストで使ったやつ
+            }                      //
+            
+            for(int i = 0; i < 4; i++)               //
+            {                                        //
+                acc[i] = omni.hMove(10,i) + control; //omni.hMove(角度radian,タイヤ番号)　で速度を返す  角度が10なら平行移動しない  必須
+            }                                        //+controlで角度がずれたら修正
+            
+            for(int i = 0; i < 4;i++)         //
+            {                                 //モーターに出力 必須
+                ikarashi[i].setSpeed(acc[i]); //
+            }                                 //
+            
+            
+            pc.printf("%f   ",angle); //ジャイロセンサの出してる角度を表示
+            
+            pc.printf("%f,%f,%f,%f\r\n",acc[0],acc[1],acc[2],acc[3]); //各モーターの出力を表示
+        }
+        else                      //ジャイロが動いていなかったら
+        {                         //
+            pc.printf("error");   //errorを表示して動かない
+        }                         //
+    }
+}