Diff: main.cpp

Revision:

26:81346a21d301

Parent:

25:ce789ea15628

Child:

27:4f2fc7172b31

--- a/main.cpp	Wed Oct 02 02:13:36 2019 +0000
+++ b/main.cpp	Tue Oct 08 02:03:01 2019 +0000
@@ -12,9 +12,6 @@
 #include "QEI.h"
 #include "PID.h"
 
-//直進補正の為の前後・左右の回転差の許容値
-#define wheel_difference    100
-
 //終了phase
 #define FINAL_PHASE 50
 
@@ -196,6 +193,8 @@
 
     init();
     init_send();
+    //後で消せ俺さん！！
+    //phase = 50;
     
     //起動時にゾーンを読んでからループに入る(試合中誤ってスイッチ押すのを防止)
     while(1) {
@@ -223,6 +222,12 @@
             i2c.write(0x30, servo_data, 1);
         }
         
+        /*
+        if(start_switch == 1) {
+            phase = 37;
+        }
+        */
+        
         //青ゾーン
         if(zone == BLUE) {
             GREEN_LED = 1;
@@ -255,13 +260,13 @@
                     i2c.write(0x30, servo_data, 1);
                     break;
 
-                //0.5秒停止
+                //1秒停止
                 case 2:
                     stop();
                     servo_data[0] = 0x04;
                     i2c.write(0x30, servo_data, 1);
                     counter.start();
-                    if(counter.read() > 0.5f) {
+                    if(counter.read() > 1.0f) {
                         phase = 3;
                         wheel_reset();
                     }
@@ -271,7 +276,7 @@
                 case 3:
                     counter.reset();
                     front(800);
-                    if((y_pulse1 > 800) || (y_pulse2 > 800)) {
+                    if((y_pulse1 > 700) || (y_pulse2 > 700)) {
                         phase = 4;
                     }
                     if(front_limit == 3) {
@@ -291,8 +296,17 @@
 
                 //回収アーム引っ込める
                 case 5:
-                    counter.reset();
-                    kaisyu_hiku(arm_pulse, 6);
+                    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;
+                    }
+                    
                     break;
 
                 //0.5秒停止
@@ -638,71 +652,32 @@
                     stop();
                     counter.start();
                     if(counter.read() > 1.0f) {
-                        phase = 38;
+                        phase = 37;
                         wheel_reset();
                     } else {
                         right_arm_data[0] = 0xFF;
-                        left_arm_data[0]  = 0xFF;
+                        left_arm_data[0]  = 0x00;
                         i2c.write(0x22, right_arm_data, 1);
                         i2c.write(0x24, left_arm_data, 1);
                         wait_us(20);
                     }
                     break;
 
-                /*
-                //カウンターリセット
+                //アームアップ
                 case 37:
                     counter.reset();
-                    counter.start();
-                    arm_up(39);
-                    phase = 38;
-                    break;
-                */
-
-                //アームアップ
-                case 38:
-                    counter.reset();
                     stop();
-                    arm_up(39);
-                    //1秒間リミットを読まずに無条件で上昇(チャタリングによる誤作動防止)
-                    /*
-                    if(counter.read() < 1.0f) {
-                        right_arm_data[0] = 0xFF;
-                        left_arm_data[0]  = 0xFF;
-                        i2c.write(0x22, right_arm_data, 1);
-                        i2c.write(0x24, left_arm_data, 1);
-                        wait_us(20);
-                    } else {
-                        arm_up(39);
-                    }
-                    */
+                    arm_up(38);
                     break;
 
                 //カウンターリセット
-                case 39:
+                case 38:
                     counter.reset();
-                    //phase = 40;
-                    phase = 42;
+                    phase = 39;
                     break;
                     
-                //1秒停止
-                case 40:
-                    stop();
-                    counter.start();
-                    if(counter.read() > 1.0f) {
-                        phase = 41;
-                        wheel_reset();
-                    }
-                    break;
-                    
-                //カウンターリセット
-                case 41:
-                    counter.reset();
-                    phase = 42;
-                    break;
-
                 //シーツを掛ける
-                case 42:
+                case 39:
                     counter.start();
 
                     //1秒間ファン送風
@@ -774,13 +749,13 @@
                     i2c.write(0x30, servo_data, 1);
                     break;
 
-                //0.5秒停止
+                //1.0秒停止
                 case 2:
                     stop();
                     servo_data[0] = 0x04;
                     i2c.write(0x30, servo_data, 1);
                     counter.start();
-                    if(counter.read() > 0.5f) {
+                    if(counter.read() > 1.0f) {
                         phase = 3;
                         wheel_reset();
                     }
@@ -807,8 +782,17 @@
 
                 //回収アーム引っ込める
                 case 5:
-                    counter.reset();
+                    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;
+                    }
+                    */
                     break;
 
                 //左移動
@@ -833,9 +817,8 @@
                 //右旋回(180°)
                 case 8:
                     counter.reset();
-                    //turn_right(975);
-                    turn_right(960);
-                    if(sum_pulse > 960) {
+                    turn_right(940);
+                    if(sum_pulse > 940) {
                         phase = 9;
                     }
                     break;
@@ -1096,8 +1079,8 @@
                 //掛けるところまで前進
                 case 30:
                     counter.reset();
-                    front(10600);
-                    if((y_pulse1 > 10600) || (y_pulse2 > 10600)) {
+                    front(9500);
+                    if((y_pulse1 > 9500) || (y_pulse2 > 9500)) {
                         phase = 31;
                         counter.start();
                     }
@@ -1129,6 +1112,12 @@
                     if(counter.read() > 1.0f) {
                         phase = 34;
                         wheel_reset();
+                    } else {
+                        right_arm_data[0] = 0xFF;
+                        left_arm_data[0]  = 0x00;
+                        i2c.write(0x22, right_arm_data, 1);
+                        i2c.write(0x24, left_arm_data, 1);
+                        wait_us(20);
                     }
                     break;
 
@@ -1141,17 +1130,9 @@
 
                 //アームアップ
                 case 35:
+                    counter.reset();
                     stop();
-                    //1秒間リミットを読まずに無条件で上昇(チャタリングによる誤作動防止)
-                    if(counter.read() < 1.0f) {
-                        right_arm_data[0] = 0xFF;
-                        left_arm_data[0]  = 0xFF;
-                        i2c.write(0x22, right_arm_data, 1);
-                        i2c.write(0x24, left_arm_data, 1);
-                        wait_us(20);
-                    } else {
-                        arm_up(36);
-                    }
+                    arm_up(36);
                     break;
 
                 //カウンターリセット
@@ -1159,26 +1140,9 @@
                     counter.reset();
                     phase = 37;
                     break;
-                    
-                //1秒停止
-                case 37:
-                    stop();
-                    counter.start();
-                    if(counter.read() > 1.0f) {
-                        phase = 38;
-                        wheel_reset();
-                    }
-                    break;
-
-                //カウンターリセット
-                case 38:
-                    counter.reset();
-                    counter.start();
-                    phase = 39;
-                    break;
 
                 //シーツを掛ける
-                case 39:
+                case 37:
                     counter.start();
 
                     //1秒間ファン送風
@@ -1230,6 +1194,7 @@
     zone_switch.mode(PullDown);
     
     YELLOW_LED = 0;
+    USR_LED3 = 0;   USR_LED4 = 0;
 
     //非常停止関連
     pic.baud(19200);
@@ -1282,7 +1247,7 @@
 }
 
 void print_pulses(void) {
-        pc.printf("%d\n\r", arm_pulse);
+        //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);
         //pc.printf("%d, %d, %d, %d, %d, %d, %d, %d, %d\n\r", front_limit, back_limit, right_limit, kaisyu_mae_limit, kaisyu_usiro_limit, 
@@ -1769,11 +1734,11 @@
     
     //両腕、上限リミットが押されてなかったら上昇
     if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 0)) {
-        right_arm_data[0] = 0xFF;   left_arm_data[0] = 0xFF;
+        right_arm_data[0] = 0xFF;   left_arm_data[0] = 0x00;
     }
     //右腕のみリミットが押されたら左腕のみ上昇
     else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 0)) {
-        right_arm_data[0] = 0x80;   left_arm_data[0] = 0xFF;
+        right_arm_data[0] = 0x80;   left_arm_data[0] = 0x00;
     }
     //左腕のみリミットが押されたら右腕のみ上昇
     else if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 1)) {