Diff: main.cpp

Revision:

9:ce5a1315fe0d

Parent:

8:123cd1f07aea

Child:

10:ebb59c1d369e

--- a/main.cpp	Mon Aug 20 13:39:26 2018 +0000
+++ b/main.cpp	Sun Aug 26 09:07:19 2018 +0000
@@ -7,56 +7,17 @@
 #include "Odometer.h"
 #include "PID.h"
 #include "RotaryEncoder.h"
-#include "CSV.h"
 #include "SBUS.h"
 #include "USS.h"
 #include "hardwareConfig.h"
+#include "stateLib.h"
 
-//(X, Y, θ, speed, angle)
-float position[19][6] = {{-1.825, 0.0, 0.0, 0.0, 0, 82},//0, x移動, 角度変化
-                         {-1.825, 0.0, 0.0, 18.0, 0, 82},//1,y移動
-                         {-1.825, 0.0, 0.0, 18.0, 35, 82},//2,発射
-                         {-1.825, 0.0, 0.0, 0.0, 0, 88},//3,y移動,角度戻す, P上昇
-                         
-                         {-2.825, 0.0, 0.0, 0.0, 0, 88},//4, x移動, 角度変化, 下降&バットマン駆動
-                         {-2.825, 0.0, 0.0, 18.0, 0, 82},//5,y移動
-                         {-2.825, 0.0, 0.0, 18.0, 35, 82},//6,発射
-                         {-2.825, 0.0, 0.0, 0.0, 0, 88},//7,y移動,角度戻す, P上昇
-                         
-                         {-3.825, 0.0, 0.0, 0.0, 0, 88},//8,x移動, 角度変化, 下降&バットマン駆動
-                         {-3.825, 0.0, 0.0, 18.0, 0, 82},//9,y移動
-                         {-3.825, 0.0, 0.0, 18.0, 35, 82},//10,発射
-                         {-3.825, 0.0, 0.0, 0.0, 0, 88},//11,y移動,角度戻す, P上昇
-                         
-                         {-0.825, 0.0, 0.0, 0.0, 0, 88},//12,x移動, 角度変化, 下降&バットマン駆動
-                         {-0.825, 0.0, 0.0, 18.0, 0, 82},//13,y移動
-                         {-0.825, 0.0, 0.0, 18.0, 18, 82},//14,下段に発射
-                         {-0.825,0.0, 0.0, 18.0, 0, 88},//15,角度戻す, P上昇
-                         {-0.825, 0.0, 0.0, 18.0, 0, 85},//16,角度変化, 下降&バットマン駆動
-                         {-0.825, 0.0, 0.0, 18.0, 18, 85},//17,上段に発射
-                         /*
-                         {-0.825, 0.0, 90.0, 0.0, 0, 88},//18
-                         
-                         {-0.5, 0.0, 90.0, 0.0, 0, 88},//19
-                         {-0.5, 1.125, 90.0, 0.0, 0, 88},//20
-                         {-0.5, 1.125, 90.0, 18.0, 0, 82},//21
-                         {-0.5, 1.125, 90.0, 18.0, 30, 82},//22
-                         {-0.5, 1.125, 90.0, 0.0, 0, 88},//23
-                         
-                         {-0.5, 1.625, 90.0, 0.0, 0, 88},//24
-                         {-0.5, 1.625, 90.0, 18.0, 0, 82},//25
-                         {-0.5, 1.625, 90.0, 18.0, 30, 82},//26
-                         {-0.5, 1.625, 90.0, 0.0, 0, 88},//27
-                         
-                         {-0.5, 2.125, 90.0, 0.0, 0, 88},//28
-                         {-0.5, 2.125, 90.0, 18.0, 0, 82},//29
-                         {-0.5, 2.125, 90.0, 18.0, 30, 82},//30
-                         {-0.5, 2.125, 90.0, 0.0, 0, 88},//31
-                         */
-                         {-0.0, 0.0, 0.0, 0, 0.0, 90}//18
-                        };
+elements getRobotVelocity(state);
+state getTargetState();
+int updateStateNum();
+bool isConvergetnceTops(int);
 
-bool isConvergetnceTops();
+controller getPropoData();
 
 //x軸補正用 PID
 PID pidRobotX(2, 0, 0, 0.01, 0.3, &timer);
@@ -67,18 +28,17 @@
 float target_y = 0;
 
 //yow角補正用 (Pgain, Igain, Dgain, 制御ループ時間[s], 計算出力100%定義)
-PID pidRobotYow(0.05, 0, 0, 0.01, 0.95, &timer);
+PID pidRobotYow(0.05, 0, 0, 0.01, 0.8, &timer);
 float target_yow = 0;
-    
+
 //USS用
 PID pidUss(0.025, 0, 0, 0.1, 0.3, &timer);
-float target_uss = 8.0;
-int posi_num = 0;
+float target_uss = 0;
 
 int main()
 {
     //タイマー３の優先度を最低にする
-    NVIC_SetPriority(TIMER3_IRQn, 100);
+    NVIC_SetPriority(TIMER3_IRQn, 3);
 
     //IMUのキャリブレーション
     wait(1);
@@ -86,21 +46,29 @@
     imu.startAngleComputing();
     
     uss.startTriger();
-
-    for(int i; i < 3; i++)
-        enc[i].changeDirection();
+    
+    enc[0].changeDirection();
+    enc[1].changeDirection();
+    enc[2].changeDirection();
 
     //オドメーターの定義
     float matrix[3][3] = {{1, 0, 0},
                           {0, 1, 0},
                           {0, 0, 0}
                          };
-    Odometer odm(matrix, 0.048);
+    Odometer odm(matrix, 0.050);
     float tmp = 0;
     float *encoders[3] = {&enc[0].rotations, &enc[1].rotations, &tmp};
     odm.setupOdometerSensors(encoders, &imu.angle[2]);
-    odm.startComputingOdometry(0.005, 0, 0, 0);
-
+    odm.startComputingOdometry(0.01, 0, 0, 0);
+    mecanum.imu_yow = &imu.angle[2];
+    
+    //許容誤差
+    pidRobotX.allowable_error = 0.1;
+    pidRobotY.allowable_error = 0.1;
+    pidRobotYow.allowable_error = 2;
+    pidUss.allowable_error = 3;
+    
     //PID設定
     pidRobotX.sensor = &odm.x;
     pidRobotX.target = &target_x;
@@ -115,115 +83,49 @@
     pidUss.target = &target_uss;
     pidUss.start();
     
-    //許容誤差
-    pidRobotX.allowable_error = 0.1;
-    pidRobotY.allowable_error = 0.1;
-    pidRobotYow.allowable_error = 2;
-    pidUss.allowable_error = 3;
-    
     //マイクロスイッチ
     sw1.mode(PullUp);
     sw2.mode(PullUp);
     
-    while(sw1 == 0);
+    odm.x = 0;
+    timer.start();
+    
     while(1)
     {
+        //controller cmd = getPropoData();   //getPropoData & getCanData
+        //float robot_velocity[3] = {cmd.LX, cmd.LY, cmd.RX};
+        
         can.read();
-        /*
-        int supply_pid = can.get(2, 1);
-        int supply_wait = can.get(2, 2);
-        int angle_wait = can.get(3, 5);
-        */
-        int sw[8] = {can.get(4, 1),
-                     can.get(4, 2),
-                     can.get(4, 3),
-                     can.get(4, 4),
-                     can.get(4, 5),
-                     can.get(4, 6),
-                     can.get(4, 7),
-                     can.get(4, 8)
-                    };
+        
+        //目指すべきロボットの状態を取得
+        state tar_state = getTargetState();
         
-        can.set(1, 1, int(position[posi_num][4]));
-        can.set(1, 2, int(position[posi_num][5]));
-        /*
-        if(supply_wait == 1 && angle_wait == 1)
-            can.set(1, 3, 1);
-        else can.set(1, 3, 0);
-        */
-        while(can.send() == 0);
+        //canに置く
+        can.set(1, 1, tar_state.shoot);
+        //角度は2倍して送って、2で割って受け取る  角度可変が0.5度刻みにできる
+        can.set(1, 2, int (tar_state.angle * 2));
+        can.set(1, 3, tar_state.supply);
         
-        //ポジション収束判定
-        if(pidRobotX.isConvergence(1) == 1 
-        && pidRobotYow.isConvergence(1) == 1)
+        //送信周期調整
+        static double pre_time = 0;
+        double now_time = timer.read();
+        if(now_time - pre_time >= 0.01)
         {
-            //超音波で近づくとき
-            if(position[posi_num][1] >= 10)
-            {
-                if(pidUss.isConvergence(1) == 1
-                && isConvergetnceTops() == 1)
-                    posi_num++;
-            }
-            //超音波使わないとき
-            else if(pidRobotY.isConvergence(1) == 1)
-            {
-                /*
-                if(posi_num == 2 || posi_num == 6 || posi_num == 10 || posi_num == 14 || posi_num == 17)
-                {
-                    if(supply_pid == 1)
-                        posi_num++;
-                }
-                else 
-                */
-                posi_num++;
-            }
-        }
-        if(sw[0] == 1 && posi_num == 0)
-            posi_num = 4;
-        if(sw[1] == 1 && posi_num == 4)
-            posi_num = 8;
-        if(sw[2] == 1 && posi_num == 8)
-            posi_num = 12;
-        if(sw[3] == 1 && sw[4] == 1 && posi_num == 12)
-            posi_num = 14;
-        if(sw[3] == 1 && posi_num == 14)
-            posi_num = 17;
-        if(sw[4] == 1 && posi_num == 17)
-            posi_num = 18;
-        
-        if(posi_num < 19)
-        {
-            posi_num = 0;
-            while(sw1 == 0);
+            while(can.send() == 0);
+            pre_time = now_time;
         }
         
-        //ロボットの移動速度(LX, LY, RX)
-        float robot_velocity[3];
-
-        //yow角調整処理
-        *pidRobotYow.target = position[posi_num][2];
-        robot_velocity[2] = pidRobotYow.output;
-
-        //x軸調整処理
-        *pidRobotX.target = position[posi_num][0];
-        robot_velocity[0] = pidRobotX.output;
-        
-        //USS距離調整処理
-        if(position[posi_num][3] >= 10)
+        if(sw2 == 1)
         {
-            *pidUss.target = position[posi_num][3] - 10;
-            robot_velocity[1] = -pidUss.output;
+            odm.x = 0;
+            odm.y = 0;
+            imu.angle[2] = 0;
         }
-        else 
-        {
-            //y軸調整処理
-            *pidRobotY.target = position[posi_num][1];
-            robot_velocity[1] = pidRobotY.output;
-        }
-        /*
-        if(sw1 == 1 && sw2 == 1)
-            imu.angle[2] = odm.y = 0;
-        */
+        
+        //ロボット速度を取得
+        elements robot_vel = getRobotVelocity(tar_state);
+        float robot_velocity[3] = {robot_vel.x, robot_vel.y, robot_vel.theta};
+        
         //ホイール速度計算
         mecanum.setVelG(robot_velocity);
         mecanum.computeWheelVel();
@@ -233,24 +135,182 @@
         for(int i = 0; i < 4; i++)
             Motor[i].drive(mecanum.wheel_vel[i]);
         
+        //pc.printf("%.2f\t", enc[0].rotations);
+        
         pc.printf("%.2f\t", odm.x);
         pc.printf("%.2f\t", odm.y);
         pc.printf("%.2f\t", imu.angle[2]);
         pc.printf("%.2f\t", uss.distance);
-        pc.printf("\n");
+        pc.printf("%.2f\t", target_yow);
+        pc.printf("%.2f\t", pidRobotYow.target);
+        pc.printf("%d\t", can.get(4, 1));
+        
         
-        wait(0.02);
+        //pc.printf("%.2f\t", enc[0].rotations);
+        /*
+        pc.printf("%.2f\t", tar_state.y);
+        pc.printf("%.2f\t", tar_state.theta);
+        pc.printf("%.2f\t", tar_state.shoot);
+        pc.printf("%.2f\t", tar_state.angle);
+        pc.printf("%.2f\t", tar_state.supply);
+        */
+        pc.printf("\n");
     }
 }
 
-bool isConvergetnceTops()
+state getTargetState()
+{
+    static bool start_flag = 0;
+        
+    int num;
+    
+    /*
+    スタートボタン待機
+    whileにするとほかの処理がすべて止めるためこれを回避
+    */
+    if(sw1 == 1)
+        start_flag = 1;
+    
+    //sw1が1度も押されていないとき、num = 0 を強制
+    if(!start_flag)
+        num = 0;
+    else num = updateStateNum();
+    
+    //num = 0 なので、再びスタート待機
+    if(num == 0)
+        start_flag = 0;    
+    
+    state a;
+    //states辞典から値を引っ張る
+    a.x = state_lib[num][0];
+    a.y = state_lib[num][1];
+    a.theta = state_lib[num][2];
+    a.shoot = state_lib[num][3];
+    a.angle = state_lib[num][4];
+    a.supply = state_lib[num][5];
+    return a;
+}
+
+int updateStateNum()
+{
+    /*
+    状態収束を判定して次のステップに進む
+    */
+    float t = 0.1;
+    static int num = 0;
+    //x方向、y方向、yow方向の判定をはっきりしておく
+    bool flag_x = 0, flag_y = 0, flag_yow = 0;
+    
+    if(pidRobotYow.isConvergence(t))
+        flag_yow = 1;
+    
+    //超音波で近づくとき
+    if(state_lib[num][1] >= 10)
+    {
+        if(pidUss.isConvergence(t+1) == 1 && isConvergetnceTops(num) == 1)
+            flag_y = 1;        
+    }
+    else if(pidRobotY.isConvergence(t) == 1)
+        flag_y = 1;    
+    
+    if(state_lib[num][0] >= 10)
+    {
+        if(pidUss.isConvergence(t+1) == 1 && isConvergetnceTops(num) == 1)
+            flag_x = 1;
+    }
+    else if(pidRobotX.isConvergence(t) == 1)
+        flag_x = 1;    
+    
+    //全部が収束してるか
+    if(flag_x && flag_y && flag_yow)
+        num++;
+    
+    //振り出しに戻る
+    if(num >= LIBNUM)
+    {
+        num = 0;
+    }
+    
+    return num;
+}
+
+elements getRobotVelocity(state a)
+{
+    elements vel;
+    
+    //yow角調整処理
+    *pidRobotYow.target = a.theta;
+    vel.theta = pidRobotYow.output;
+    
+    
+    //USS距離調整処理
+    if(a.x >= 10)
+    {
+        *pidUss.target = a.x - 10;
+        vel.x = -pidUss.output;
+    }
+    else 
+    {
+        *pidRobotX.target = a.x;
+        vel.x = pidRobotX.output;
+    }
+    
+    //USS距離調整処理
+    if(a.y >= 10)
+    {
+        *pidUss.target = a.y - 10;
+        vel.y = -pidUss.output;
+    }
+    else 
+    {
+        //y軸調整処理
+        *pidRobotY.target = a.y;
+        vel.y = pidRobotY.output;
+    }
+    
+    return vel;
+}
+bool isConvergetnceTops(int state_num)
 {
     int velocity_pid = can.get(3, 1);
     int angle_pid    = can.get(3, 2);
     int velocity_val = can.get(3, 3);
-    int angle_val    = can.get(3, 4);
+    float angle_val    = can.get(3, 4);
     
-    if(angle_pid == 1 && velocity_pid == 1 && velocity_val == position[posi_num][3] && angle_val == position[posi_num][4])
+    angle_val /= 2.0;
+    if(angle_pid == 1 && velocity_pid == 1 && velocity_val == state_lib[state_num][3] && angle_val == state_lib[state_num][4])
         return 1;
     else return 0;
-}
\ No newline at end of file
+}
+
+controller getPropoData()
+{
+    float dead_zone = 0.05;
+    controller propo;
+    sbus.isFailSafe();
+    
+    //propo直接コントロール
+    if(sbus.isFailSafe())
+    {
+        propo.LX = propo.LY = propo.RX = propo.RY = 0;
+        propo.H = propo.A = propo.D = propo.F = propo.G = 0;
+    }
+    else
+    {
+        propo.LX = sbus.getStickVal(0) / 255.0;
+        propo.LY = sbus.getStickVal(1) / 255.0;
+        propo.RX = -sbus.getStickVal(2) / 255.0;
+        propo.RY = sbus.getStickVal(3) / 255.0;
+        propo.H = sbus.getSwitchVal(0);
+        propo.A = sbus.getSwitchVal(1);
+        propo.D = sbus.getSwitchVal(2);
+        propo.F = sbus.getSwitchVal(3);
+        propo.G = sbus.getSwitchVal(4);
+    }
+    
+    if(propo.RX < dead_zone && propo.RX > -dead_zone)  propo.RX = 0;
+    if(propo.RY < dead_zone && propo.RY > -dead_zone)  propo.RY = 0;
+    if(propo.LX < dead_zone && propo.LX > -dead_zone)  propo.LX = 0;
+    if(propo.LY < dead_zone && propo.LY > -dead_zone)  propo.LY = 0;
+    return propo;
+}