Diff: main.cpp

Revision:

27:4f2fc7172b31

Parent:

26:81346a21d301

Child:

28:e95db716197d

--- a/main.cpp	Tue Oct 08 02:03:01 2019 +0000
+++ b/main.cpp	Sat Oct 19 01:10:39 2019 +0000
@@ -2,7 +2,7 @@
 /* NHK ROBOCON 2019 Ibaraki Kosen A team Automatic                      */
 /* Nucleo Type: F446RE                                                  */
 /* designed by Yuhi Takaku from 5D, mail: rab1sy23@gmail.com            */
-/* Actuator: RS-555*4, RS-380*2, RZ-735*2, RS-385*2, PWM_Servo(KONDO)*2 */
+/* Actuator: RS-555*4, RS-385*4, RZ-735*2, PWM_Servo(KONDO)*2           */
 /* Sensor: encorder*4, limit_switch*14                                  */
 /* -------------------------------------------------------------------  */
 /* Both of areas are compleated!                                        */
@@ -18,6 +18,9 @@
 #define RED     0
 #define BLUE    1
 
+#define wind_time1  1.00f
+#define wind_time2  1.25f
+
 //PID Gain of wheels(Kp, Ti, Td, control cycle)
 //前進
 PID front_migimae(4500000.0, 0.0, 0.0, 0.001);
@@ -99,8 +102,8 @@
 
 //操作の段階変数
 unsigned int phase = 0;
-int kaisyu_phase = 0;
-int tyokudo_phase = 0;
+unsigned int kaisyu_phase = 0;
+unsigned int tyokudo_phase = 0;
 unsigned int start_zone = 1;
 bool zone = RED;
 
@@ -170,10 +173,11 @@
 void read_limit(void);
 void wheel_reset(void);
 void kaisyu(int pulse, int next_phase);
-void kaisyu_nobasu(int pulse, int next_phase);
-void kaisyu_hiku(int pulse, int next_phase);
+void kaisyu_nobasu(int next_phase);
+void kaisyu_hiku(int next_phase);
 void tyokudo(int pulse, int next_phase);
 void arm_up(int next_phase);
+void fan_on(float first_wind_time, float second_wind_time, int next_phase);
 void front(int target);
 void back(int target);
 void right(int target);
@@ -224,7 +228,14 @@
         
         /*
         if(start_switch == 1) {
-            phase = 37;
+            //phase = 37;
+            if(zone == RED) {
+                phase = 35;
+                //phase = 37;
+            }
+            else if(zone == BLUE) {
+                phase = 37;
+            }
         }
         */
         
@@ -254,7 +265,7 @@
                 //回収アームを伸ばす
                 case 1:
                     counter.reset();
-                    kaisyu_nobasu(arm_pulse, 2);
+                    kaisyu_nobasu(2);
                     //サーボを開いておく
                     servo_data[0] = 0x03;
                     i2c.write(0x30, servo_data, 1);
@@ -275,7 +286,7 @@
                 //ちょっと前進
                 case 3:
                     counter.reset();
-                    front(800);
+                    front(700);
                     if((y_pulse1 > 700) || (y_pulse2 > 700)) {
                         phase = 4;
                     }
@@ -297,21 +308,14 @@
                 //回収アーム引っ込める
                 case 5:
                     USR_LED3 = 1;
-                    //kaisyu_hiku(arm_pulse, 6);
-                    
-                    if(kaisyu_mae_limit == 0) {
-                        arm_motor[0] = 0x4C;
-                    }
-                    else if(kaisyu_mae_limit == 1) {
-                        arm_motor[0] = 0x80;
-                        phase = 6;
-                    }
-                    
+                    counter.reset();
+                    kaisyu_hiku(6);
                     break;
 
                 //0.5秒停止
                 case 6:
                     stop();
+                    USR_LED4 = 1;
                     counter.start();
                     if(counter.read() > 0.5f) {
                         phase = 7;
@@ -621,8 +625,8 @@
                 //掛けるところまで後進
                 case 33:
                     counter.reset();
-                    back(-9500);
-                    if((y_pulse1*-1 > 9500) || (y_pulse2*-1 > 9500)) {
+                    back(-9200);
+                    if((y_pulse1*-1 > 9200) || (y_pulse2*-1 > 9200)) {
                         phase = 34;
                         counter.start();
                     }
@@ -679,7 +683,9 @@
                 //シーツを掛ける
                 case 39:
                     counter.start();
-
+                    //fan_on(1.0f, 1.75f, FINAL_PHASE);
+                    fan_on(wind_time1, wind_time2, FINAL_PHASE);
+                    /*
                     //1秒間ファン送風
                     if(counter.read() <= 1.0f) {
                         fan_data[0] = 0xFF;
@@ -705,6 +711,7 @@
                         i2c.write(0x30, servo_data, 1);
                         phase = FINAL_PHASE;
                     }
+                    */
                     break;
 
                 //終了っ！(守衛さん風)
@@ -743,7 +750,7 @@
                 //回収アームを伸ばす
                 case 1:
                     counter.reset();
-                    kaisyu_nobasu(arm_pulse, 2);
+                    kaisyu_nobasu(2);
                     //サーボを開いておく
                     servo_data[0] = 0x03;
                     i2c.write(0x30, servo_data, 1);
@@ -783,20 +790,12 @@
                 //回収アーム引っ込める
                 case 5:
                     USR_LED3 = 1;
-                    kaisyu_hiku(arm_pulse, 6);
-                    /*
-                    if(kaisyu_mae_limit == 0) {
-                        arm_motor[0] = 0x4C;
-                    }
-                    else if(kaisyu_mae_limit == 1) {
-                        arm_motor[0] = 0x80;
-                        phase = 6;
-                    }
-                    */
+                    kaisyu_hiku(6);
                     break;
 
                 //左移動
                 case 6:
+                    USR_LED4 = 1;
                     counter.reset();
                     left(11500);
                     if((x_pulse1 > 11500) || (x_pulse2 > 11500)) {
@@ -1144,7 +1143,10 @@
                 //シーツを掛ける
                 case 37:
                     counter.start();
+                    //fan_on(1.0f, 1.75f, FINAL_PHASE);
+                    fan_on(wind_time1, wind_time2, FINAL_PHASE);
 
+                    /*
                     //1秒間ファン送風
                     if(counter.read() <= 1.0f) {
                         fan_data[0] = 0xFF;
@@ -1170,6 +1172,7 @@
                         i2c.write(0x30, servo_data, 1);
                         phase = FINAL_PHASE;
                     }
+                    */
                     break;
 
                 //終了っ！(守衛さん風)
@@ -1247,6 +1250,7 @@
 }
 
 void print_pulses(void) {
+        
         //pc.printf("p: %d, kp: %d, pulse: %d\n\r", phase, kaisyu_phase, arm_pulse);
         //pc.printf("p: %d, k_p: %d, pulse: %d\n\r", phase, kaisyu_phase, arm_pulse);
         //pc.printf("X1: %d, X2: %d, Y1: %d, Y2: %d, sum: %d\n\r", abs(x_pulse1), x_pulse2, abs(y_pulse1), y_pulse2, sum_pulse);
@@ -1572,10 +1576,12 @@
     
     //回収MDへ書き込み
     i2c.write(0x18, arm_motor, 1);
+    wait_us(20);
 }
 
-void kaisyu_nobasu(int pulse, int next_phase) {
+void kaisyu_nobasu(int next_phase) {
     
+    /*
     switch (kaisyu_phase) {
         case 0:
             //前進->減速
@@ -1599,8 +1605,8 @@
             //3600pulse超えたら停止
             else if(pulse >= 3600) {
                 arm_motor[0] = 0x80;
+                kaisyu_phase = 2;
                 phase = next_phase;
-                kaisyu_phase = 2;
             }
             //後ろのリミットが押されたら強制停止
             if(kaisyu_usiro_limit == 1) {
@@ -1614,12 +1620,39 @@
             arm_motor[0] = 0x80;
             break;    
     }
+    */
+    
+    //前進->減速
+    //3000pulseまで高速前進
+    if(arm_pulse < 3000) {
+        arm_motor[0] = 0xFF;
+    }
+    //3000pulse超えたら低速前進
+    else if(arm_pulse >= 3000) {
+        arm_motor[0] = 0xB3;
+    }
+    //3600pulse超えたら停止
+    else if(arm_pulse >= 3600) {
+        arm_motor[0] = 0x80;
+        phase = next_phase;
+    } else {
+        arm_motor[0] = 0x80;
+    }
+    
+    //後ろのリミットが押されたら強制停止
+    if(kaisyu_usiro_limit == 1) {
+        arm_motor[0] = 0x80;
+        phase = next_phase;
+    }
+    
     //回収MDへ書き込み
     i2c.write(0x18, arm_motor, 1);
+    wait_us(20);
 }
 
-void kaisyu_hiku(int pulse, int next_phase) {
+void kaisyu_hiku(int next_phase) {
     
+    /*
     switch(kaisyu_phase) {
         case 2:
             //後進->減速
@@ -1653,8 +1686,34 @@
             arm_motor[0] = 0x80;
             break;
     }
+    */
+    
+    //後進->減速
+    //500pulseより大きい範囲で高速後進
+    if(arm_pulse > 500) {
+        arm_motor[0] = 0x00;
+    }
+    //500pulse以下になったら低速後進
+    else if(arm_pulse <= 500) {
+        arm_motor[0] = 0x4C;
+    }
+    //0pulse以下で停止
+    else if(arm_pulse <= 0) {
+        arm_motor[0] = 0x80;
+        phase = next_phase;
+    } else {
+        arm_motor[0] = 0x80;
+    }
+    
+    //後ろのリミットが押されたら強制停止
+    if(kaisyu_mae_limit == 1) {
+        arm_motor[0] = 0x80;
+        phase = next_phase;
+    }
+    
     //回収MDへ書き込み
     i2c.write(0x18, arm_motor, 1);
+    wait_us(20);
 }
 
 void tyokudo(int pulse, int next_phase) {
@@ -1728,6 +1787,7 @@
     //回収MD・排出MDへ書き込み
     i2c.write(0x18, arm_motor,  1);
     i2c.write(0x20, drop_motor, 1);
+    wait_us(20);
 }
 
 void arm_up(int next_phase) {
@@ -1749,12 +1809,32 @@
         right_arm_data[0] = 0x80;   left_arm_data[0] = 0x80;
         phase = next_phase;
     }
-    
     i2c.write(0x22, right_arm_data, 1);
     i2c.write(0x24, left_arm_data, 1);
     wait_us(20);
 }
 
+void fan_on(float first_wind_time, float second_wind_time, int next_phase) {
+    
+    if(counter.read() <= first_wind_time) {
+        fan_data[0] = 0xFF;
+        servo_data[0] = 0x04;
+    }
+    else if((counter.read() > first_wind_time) && (counter.read() <= first_wind_time + second_wind_time)) {
+        fan_data[0] = 0xFF;
+        servo_data[0] = 0x03;
+    }
+    else if(counter.read() > first_wind_time + second_wind_time) {
+        fan_data[0] = 0x80;
+        servo_data[0] = 0x04;
+        phase = next_phase;
+    }
+    i2c.write(0x26, fan_data, 1);
+    i2c.write(0x28, fan_data, 1);
+    i2c.write(0x30, servo_data, 1);
+    wait_us(20);
+}
+
 void front(int target) {
 
         front_PID(target);