Diff: main.cpp

Revision:

18:268ab2ab0b2a

Parent:

13:0479a4f3e997

Child:

19:bdb503dd1e8c

--- a/main.cpp	Sat Sep 15 06:48:13 2018 +0000
+++ b/main.cpp	Thu Sep 27 05:28:37 2018 +0000
@@ -15,7 +15,8 @@
 elements getRobotVelocity(state);
 state getTargetState();
 int updateStateNum();
-bool isConvergenceTops(int);
+int updateTopsNum(bool);
+bool isConvergenceCatapult(int);
 bool isConvergenceSupply(int);
 controller getPropoData();
 void riseUssTriger();
@@ -96,19 +97,14 @@
         riseUssTriger();
         can.read();
         
-        int sw[9];
-        for(int i = 0; i < 9; i++)
-            sw[i] = can.get(2, i + 1);
-        if(sw[8] == 0)
-            changeToBlueZone();
-        
         //目指すべきロボットの状態を取得
         state tar_state = getTargetState();
         
+        int can_angle = 900 - int(tar_state.angle * 10);
         //canに置く
         //角度は2倍して送って、2で割って受け取る  角度可変が0.5度刻みにできる
         can.set(1, 1, tar_state.shoot);
-        can.set(1, 2, int (tar_state.angle * 2));
+        can.set(1, 2, int(can_angle));
         can.set(1, 3, tar_state.supply);
         
         //送信周期調整
@@ -128,7 +124,7 @@
         mecanum.setVelG(robot_velocity);
         mecanum.computeWheelVel();
         mecanum.rescaleWheelVel();
-
+        
         //モーターの駆動
         for(int i = 0; i < 4; i++)
             Motor[i].drive(mecanum.wheel_vel[i]);
@@ -137,22 +133,17 @@
         pc.printf("%.2f\t", odm.y);
         pc.printf("%.2f\t", imu.angle[2]);
         
-        int s1 = sw1, s2 = sw2;
-        pc.printf("%d\t", s1);
-        pc.printf("%d\t", s2);
-        
-        pc.printf("%.2f\t", uss[0].distance);
-        pc.printf("%.2f\t", uss[1].distance);
-        
         pc.printf("\n");
     }
 }
 
 state getTargetState()
 {
+    static bool have_ever_shot = 0;
     static bool start_flag = 0;
-        
-    int num;
+    
+    int state_num;
+    int tops_num;
     
     /*
     スタートボタン待機
@@ -163,24 +154,42 @@
     
     //sw1が1度も押されていないとき
     if(!start_flag)
-        num = odm.x = odm.y = imu.angle[2] = 0;
-    else num = updateStateNum();
+    {
+        state_num = odm.x = odm.y = imu.angle[2] = have_ever_shot = 0;
+    }
+    else state_num = updateStateNum();
     
     //再びスタート待機
-    if(num == 0)
-        start_flag = 0;    
+    if(state_num == 0)
+        start_flag = 0;   
+    
+    int target_table = state_lib[state_num][3];
+    bool is_shoot = state_lib[state_num][4];
+    
+    have_ever_shot |= is_shoot;
+    
+    if(have_ever_shot)
+        tops_lib[1][2] = 1;
+    else tops_lib[1][2] = 0;
+    
+    tops_lib[3][0] = para_lib[target_table].vel;
+    tops_lib[2][1] = tops_lib[3][1] = para_lib[target_table].angle;
+
+    tops_num = updateTopsNum(is_shoot);
     
     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];
+    a.x = state_lib[state_num][0];
+    a.y = state_lib[state_num][1];
+    a.theta = state_lib[state_num][2];
+    
+    a.shoot = tops_lib[tops_num][0];
+    a.angle = tops_lib[tops_num][1];
+    a.supply = tops_lib[tops_num][2];
     return a;
 }
 
+bool is_shot = 0;
+
 int updateStateNum()
 {
     /*
@@ -189,14 +198,14 @@
     float t = 0.1;
     static int num = 0;
     //x方向、y方向、yow方向の判定をはっきりしておく
-    bool flag_x = 0, flag_y = 0, flag_yow = 0;
+    bool flag_x = 0, flag_y = 0, flag_yow = 0, flag_tops = 0;
     
     if(pidRobotYow.isConvergence(t))
         flag_yow = 1;
     
     if(state_lib[num][0] >= 10 || state_lib[num][0] <= -10)
     {
-        if(pidUss.isConvergence(t+1) && isConvergenceTops(num) && isConvergenceSupply(num))
+        if(pidUss.isConvergence(t+1))
             flag_x = flag_y = 1;
     }
     else if(pidRobotX.isConvergence(t) == 1)
@@ -205,24 +214,82 @@
     //超音波で近づくとき
     if(state_lib[num][1] >= 10 || state_lib[num][1] <= -10)
     {
-        if(pidUss.isConvergence(t+1) && isConvergenceTops(num) && isConvergenceSupply(num))
+        if(pidUss.isConvergence(t+1))
             flag_y = flag_x = 1;
     }
     else if(pidRobotY.isConvergence(t) == 1)
         flag_y = 1;
     
+    if(state_lib[num][4] == 1)
+    {
+        if(is_shot)
+        {
+            flag_tops = 1;
+            is_shot = 0;
+        }
+    }
+    else flag_tops = 1;
     
     //全部が収束してるか
-    if(flag_x && flag_y && flag_yow)
+    if(flag_x && flag_y && flag_yow && flag_tops)
         num++;
     
     //振り出しに戻る
     if(num >= LIBNUM)
         num = 0;
     
+    pc.printf("%d\t", num);
+    pc.printf("%d\t", flag_x);
+    pc.printf("%d\t", flag_y);
+    pc.printf("%d\t", flag_yow);
+    pc.printf("%d\t", flag_tops);
+        
     return num;
 }
 
+int updateTopsNum(bool is_shoot)
+{
+    static int num = 0;
+    if(isConvergenceCatapult(num) && isConvergenceSupply(num))
+    {
+        if(num != 2)
+            num++;
+        else if(is_shoot)
+            num++;
+    }
+    if(num >= 4)
+    {
+        num = 0;
+        is_shot = 1;
+    }
+    return num;
+}
+
+bool isConvergenceCatapult(int num)
+{
+    int velocity_pid = can.get(3, 1);
+    int angle_pid    = can.get(3, 2);
+    int velocity_val = can.get(3, 3);
+    float angle_val    = can.get(3, 4);
+    angle_val = 90 - (angle_val / 10.0);
+        
+    if(angle_pid == 1 && velocity_pid == 1 && velocity_val == tops_lib[num][0] && angle_val == tops_lib[num][1])
+        return 1;
+    else return 0;
+}
+
+bool isConvergenceSupply(int num)
+{
+    int is_supply_done = can.get(3, 5);
+        
+    if(tops_lib[num][2] == 1)
+    {
+        if(is_supply_done == 1)
+            return 1;
+        else return 0;
+    }else return 1;
+}
+
 elements getRobotVelocity(state a)
 {
     elements vel;
@@ -290,34 +357,12 @@
         *pidUss.target = 0; 
         *pidRobotX.target = a.x;
         *pidRobotY.target = a.y;
-         vel.x = pidRobotX.output;
+        vel.x = pidRobotX.output;
         vel.y = pidRobotY.output;
     }
     return vel;
 }
-bool isConvergenceTops(int state_num)
-{
-    int velocity_pid = can.get(3, 1);
-    int angle_pid    = can.get(3, 2);
-    int velocity_val = can.get(3, 3);
-    float angle_val    = can.get(3, 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;
-}
-bool isConvergenceSupply(int state_num)
-{
-    int is_supply_done = can.get(3, 5);
-        
-    if(state_lib[state_num][5] == 1)
-    {
-        if(is_supply_done == 1)
-            return 1;
-        else return 0;
-    }else return 1;
-}
+
 controller getPropoData()
 {
     float dead_zone = 0.05;