Diff: main.cpp

Revision:

21:4912835805a8

Parent:

20:1957c67ab740

Child:

22:2969cf94e671

--- a/main.cpp	Sun Oct 07 08:10:13 2018 +0000
+++ b/main.cpp	Wed Oct 10 12:11:50 2018 +0000
@@ -21,15 +21,15 @@
 bool isConvergenceSupply(int);
 controller getPropoData();
 void riseUssTriger();
-void caliblationImu();
-void resetOdometer();
+void reset_System();
+void discoveryTable();
 
 //x軸補正用 PID
-PID pidRobotX(4, 0, 0, 0.01, 0.4, &timer);
+PID pidRobotX(4, 0, 0, 0.01, 0.3, &timer);
 float target_x = 0;
 
 //y軸補正用 PID
-PID pidRobotY(2, 0, 0, 0.01, 0.4, &timer);
+PID pidRobotY(2, 0, 0, 0.01, 0.45, &timer);
 float target_y = 0;
 
 //yow角補正用 (Pgain, Igain, Dgain, 制御ループ時間[s], 計算出力100%定義)
@@ -37,7 +37,7 @@
 float target_yow = 0;
 
 //USS用
-PID pidUss(0.025, 0, 0, 0.1, 0.3, &timer);
+PID pidUss(0.025, 0, 0, 0.1, 0.45, &timer);
 float target_uss = 0;
 
 //オドメーターの定義
@@ -56,6 +56,11 @@
     //タイマー３の優先度を最低にする
     NVIC_SetPriority(TIMER3_IRQn, 3);
     
+    wait(1);
+    imu.performCalibration();
+    imu.startAngleComputing();
+    LED[1] = 1;
+    
     enc[0].changeDirection();
     enc[2].changeDirection();
     
@@ -95,13 +100,16 @@
     
     while(1)
     {
-        caliblationImu();
+        reset_System();
         riseUssTriger();
         can.read();
         
         //目指すべきロボットの状態を取得
         state tar_state = getTargetState();
         
+        //横USSでC_tableマップリング
+        discoveryTable();
+        
         int can_angle = 900 - int(tar_state.angle * 10);
         //角度は2倍して送って、2で割って受け取る  角度可変が0.5度刻みにできる
         can.set(1, 1, tar_state.shoot);
@@ -135,6 +143,9 @@
         
         //pc.printf("%.2f\t", odm.x);
         pc.printf("%.2f\t", odm.y);
+        pc.printf("%.2f\t", state_lib[21][1]);
+        pc.printf("%.2f\t", state_lib[25][1]);
+        
         /*
         pc.printf("%.2f\t", imu.angle[2]);
         pc.printf("%.2f\t", uss[0].distance);
@@ -239,6 +250,8 @@
     //超音波で近づくとき
     if(state_lib[num][1] >= 10 || state_lib[num][1] <= -10)
     {
+        //if(state_lib[num][4] == 1 && (uss[0].distance > 100 || uss[1].distance > 100))
+            
         if(pidUss.isConvergence(t+1))
             flag_y = flag_x = 1;
     }
@@ -339,10 +352,10 @@
     {
         if(table_sw[0] == 0 && num > 30 && num < 35)
             num = 35;
-        if(table_sw[1] == 0 && num < 34 && num > 39)
+        if(table_sw[1] == 0 && num > 34 && num < 39)
             num = 39;
         if(table_sw[2] == 0 && num > 38 && num < 43)
-            num = 42;
+            num = 43;
         if(table_sw[1] == 0 && table_sw[2] == 0 && num == 43)
             num = 44;
     }
@@ -503,43 +516,48 @@
     }
 }
 
-void caliblationImu()
+void reset_System()
 {
     int caliblation_sw = sw2;
-    static int last_sw = 0;
+    static int last_sw = 1;
     if(caliblation_sw == 1 && last_sw == 0)
-    {
-        wait(1);
-        imu.performCalibration();
-        imu.startAngleComputing();
-        last_sw = LED[1] = 1;
-    }
+        NVIC_SystemReset();
     else if(caliblation_sw == 0)
         last_sw = LED[1] = 0;
 }
 
-void resetOdometer()
+void discoveryTable()
 {
-    static double start_time = timer.read();
-   
-    static float pre_odm = odm.y;
-    
-    if(state_lib[now_num][1] != 0 || odm.y >0.5)
+    static bool c2_discovery = 0;
+    static bool c3_discovery = 0;
+    int _uss;
+    if(table_sw[8] == 0)
+        _uss = can.get(8, 1);
+    else _uss = can.get(8, 2);
+    if(now_num == 19)
     {
-        pre_odm = odm.y;
-        start_time = timer.read();
-        return;
-    }
-
-    if(odm.y - pre_odm < -0.02)  
-    { 
-        start_time = timer.read();
-        pre_odm = odm.y;
-    }
-    
-    if(timer.read() - start_time > 0.4)
-    {
-        if(odm.y < 0.2)
-            odm.y = 0;
+        if(table_sw[6] == 1)
+        {
+            if(_uss > 40.0 && _uss < 60.0)
+            {
+                state_lib[21][1] = state_lib[22][1] = odm.y - 0.7;
+                c2_discovery = 1;
+                if(c3_discovery == 0)
+                {
+                    state_lib[25][1] = state_lib[26][1] = odm.y - 0.7;
+                    c3_discovery = 1;
+                }
+            }
+            else if(c2_discovery == 0)
+                state_lib[21][1] = state_lib[22][1] = odm.y - 0.3;
+        }
+        if(table_sw[7] == 1 && c2_discovery == 0)
+        {
+            if(_uss > 120.0 && _uss < 160.0)
+            {
+                state_lib[25][1] = state_lib[26][1] = odm.y - 0.7;
+                c3_discovery = 1;
+            }
+        }
     }
 }
\ No newline at end of file