Diff: main.cpp

Revision:

20:1957c67ab740

Parent:

19:bdb503dd1e8c

Child:

21:4912835805a8

--- a/main.cpp	Wed Oct 03 08:10:17 2018 +0000
+++ b/main.cpp	Sun Oct 07 08:10:13 2018 +0000
@@ -21,13 +21,15 @@
 bool isConvergenceSupply(int);
 controller getPropoData();
 void riseUssTriger();
+void caliblationImu();
+void resetOdometer();
 
 //x軸補正用 PID
-PID pidRobotX(4, 0, 0, 0.01, 0.3, &timer);
+PID pidRobotX(4, 0, 0, 0.01, 0.4, &timer);
 float target_x = 0;
 
 //y軸補正用 PID
-PID pidRobotY(2, 0, 0, 0.01, 0.3, &timer);
+PID pidRobotY(2, 0, 0, 0.01, 0.4, &timer);
 float target_y = 0;
 
 //yow角補正用 (Pgain, Igain, Dgain, 制御ループ時間[s], 計算出力100%定義)
@@ -46,22 +48,17 @@
 Odometer odm(matrix, 0.050);
 
 int table_sw[9];
+int now_num;
 
 int main()
 {
     pc.baud(115200);
     //タイマー３の優先度を最低にする
     NVIC_SetPriority(TIMER3_IRQn, 3);
-
-    //IMUのキャリブレーション
-    wait(1);
-    imu.performCalibration();
-    imu.startAngleComputing();
     
     enc[0].changeDirection();
-    //enc[1].changeDirection();
     enc[2].changeDirection();
-
+    
     float tmp = 0;
     float *encoders[3] = {&enc[0].rotations, &enc[1].rotations, &tmp};
     odm.setupOdometerSensors(encoders, &imu.angle[2]);
@@ -98,6 +95,7 @@
     
     while(1)
     {
+        caliblationImu();
         riseUssTriger();
         can.read();
         
@@ -105,7 +103,6 @@
         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(can_angle));
@@ -114,7 +111,7 @@
         //送信周期調整
         static double pre_time = 0;
         double now_time = timer.read();
-        if(now_time - pre_time >= 0.01)
+        if(now_time - pre_time >= 0.03)
         {
             if(can.send())
             {
@@ -135,11 +132,11 @@
         //モーターの駆動
         for(int i = 0; i < 4; i++)
             Motor[i].drive(mecanum.wheel_vel[i]);
-        /*
-        pc.printf("%.2f\t", odm.x);
+        
+        //pc.printf("%.2f\t", odm.x);
         pc.printf("%.2f\t", odm.y);
+        /*
         pc.printf("%.2f\t", imu.angle[2]);
-        
         pc.printf("%.2f\t", uss[0].distance);
         pc.printf("%.2f\t", uss[1].distance);
         pc.printf("%.2f\t", enc[0].rotations);
@@ -170,7 +167,7 @@
     //sw1が1度も押されていないとき
     if(!start_flag)
         state_num = tops_num = odm.x = odm.y = imu.angle[2] = have_ever_shot = 0;
-    else state_num = updateStateNum();
+    else state_num  = now_num = updateStateNum();
     
     //再びスタート待機
     if(state_num == 0)
@@ -266,7 +263,7 @@
         num = 0;
     
     num = changeNextTable(num);
-    
+    /*
     pc.printf("%d\t", flag_x);
     pc.printf("%d\t", flag_y);
     pc.printf("%d\t", flag_yow);
@@ -274,7 +271,7 @@
     pc.printf("%d\t", pidRobotX.isConvergence(0.1));
     pc.printf("%d\t", pidRobotY.isConvergence(0.1));
     pc.printf("%d\t", pidRobotYow.isConvergence(0.1));
-    
+    */
     return num;
 }
 
@@ -302,6 +299,7 @@
     
     return num;
 }
+
 int changeNextTable(int num)
 {
     if(table_sw[3] == 1 && table_sw[4] == 0)
@@ -326,7 +324,7 @@
     if(table_sw[5] == 0 && table_sw[6] == 0 && table_sw[7] == 0)
     {
         if(num > 16 && num < 30)
-            num = 30;
+            num = 31;
     }
     else
     {
@@ -503,4 +501,45 @@
         }
         start_time = now_time;
     }
+}
+
+void caliblationImu()
+{
+    int caliblation_sw = sw2;
+    static int last_sw = 0;
+    if(caliblation_sw == 1 && last_sw == 0)
+    {
+        wait(1);
+        imu.performCalibration();
+        imu.startAngleComputing();
+        last_sw = LED[1] = 1;
+    }
+    else if(caliblation_sw == 0)
+        last_sw = LED[1] = 0;
+}
+
+void resetOdometer()
+{
+    static double start_time = timer.read();
+   
+    static float pre_odm = odm.y;
+    
+    if(state_lib[now_num][1] != 0 || odm.y >0.5)
+    {
+        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;
+    }
 }
\ No newline at end of file