Diff: main.cpp

Revision:

24:d12bc20c01c2

Parent:

23:1e4d7540715f

Child:

25:ce789ea15628

--- a/main.cpp	Sat Sep 28 01:24:03 2019 +0000
+++ b/main.cpp	Sun Sep 29 17:01:17 2019 +0000
@@ -5,7 +5,7 @@
 /* Actuator: RS-555*4, RS-380*2, RZ-735*2, RS-385*2, PWM_Servo(KONDO)*2 */
 /* Sensor: encorder*4, limit_switch*14                                  */
 /* -------------------------------------------------------------------  */
-/* added red zone(checked)                                              */
+/* Both of areas are compleated!                                        */
 /* -------------------------------------------------------------------  */
 #include "mbed.h"
 #include "math.h"
@@ -79,6 +79,7 @@
 DigitalOut USR_LED4(PC_3);
 DigitalOut  GREEN_LED(D8);
 DigitalOut  RED_LED(D10);
+DigitalOut  YELLOW_LED(D9);
 
 //遠隔非常停止ユニットLED
 AnalogOut myled(A2);
@@ -150,17 +151,17 @@
 //吐き出し機構の下限
 bool tyokudo_usiro_limit = 0;
 
-int masked_lower_front_limit_data     = 0xFF;
-int masked_lower_back_limit_data      = 0xFF;
-int masked_lower_right_limit_data     = 0xFF;
-int masked_kaisyu_mae_limit_data      = 0xFF;
-int masked_kaisyu_usiro_limit_data    = 0xFF;
-int masked_right_arm_lower_limit_data = 0xFF;
-int masked_right_arm_upper_limit_data = 0xFF;
-int masked_left_arm_lower_limit_data  = 0xFF;
-int masked_left_arm_upper_limit_data  = 0xFF;
-int masked_tyokudo_mae_limit_data     = 0xFF;
-int masked_tyokudo_usiro_limit_data   = 0xFF;
+int masked_lower_front_limit_data     = 0b11111111;
+int masked_lower_back_limit_data      = 0b11111111;
+int masked_lower_right_limit_data     = 0b11111111;
+int masked_kaisyu_mae_limit_data      = 0b11111111;
+int masked_kaisyu_usiro_limit_data    = 0b11111111;
+int masked_right_arm_lower_limit_data = 0b11111111;
+int masked_right_arm_upper_limit_data = 0b11111111;
+int masked_left_arm_lower_limit_data  = 0b11111111;
+int masked_left_arm_upper_limit_data  = 0b11111111;
+int masked_tyokudo_mae_limit_data     = 0b11111111;
+int masked_tyokudo_usiro_limit_data   = 0b11111111;
 
 //関数のプロトタイプ宣言
 void init(void);
@@ -229,7 +230,7 @@
         
         /*
         if(start_switch == 1) {
-            phase = 39;
+            phase = 15;
         }
         */
         
@@ -315,8 +316,8 @@
                 //左移動
                 case 7:
                     counter.reset();
-                    left(11500);
-                    if((x_pulse1 > 11500) || (x_pulse2 > 11500)) {
+                    left(10500);
+                    if((x_pulse1 > 10500) || (x_pulse2 > 10500)) {
                         phase = 8;
                     }
                     break;
@@ -497,6 +498,7 @@
 
                 //シーツ装填
                 case 22:
+                    YELLOW_LED = 1;
                     if(start_switch == 1) {
                         wheel_reset();
                         phase = 23;
@@ -736,7 +738,7 @@
                     servo_data[0] = 0x04;
                     i2c.write(0x30, servo_data, 1);
                     counter.start();
-                    if(counter.read() > 1.0f) {
+                    if(counter.read() > 0.5f) {
                         phase = 3;
                         wheel_reset();
                     }
@@ -747,7 +749,8 @@
                     counter.reset();
                     front(800);
                     if((y_pulse1 > 800) || (y_pulse2 > 800)) {
-                        phase = 4;
+                        //phase = 4;
+                        phase = 5;
                     }
                     break;
                     
@@ -755,7 +758,7 @@
                 case 4:
                     stop();
                     counter.start();
-                    if(counter.read() > 1.0f) {
+                    if(counter.read() > 0.5f) {
                         phase = 5;
                         wheel_reset();
                     }
@@ -764,18 +767,9 @@
                 //回収アーム引っ込める
                 case 5:
                     counter.reset();
-                    kaisyu_hiku(arm_pulse, 6);
-                    break;
-                    
-                //1秒停止
-                case 6:
-                    stop();
-                    counter.start();
-                    if(counter.read() > 1.0f) {
-                        phase = 7;
-                        wheel_reset();
-                    }
-                    break;        
+                    //kaisyu_hiku(arm_pulse, 6);
+                    kaisyu_hiku(arm_pulse, 7);
+                    break;       
                         
                 //左移動
                 case 7:
@@ -799,8 +793,9 @@
                 //右旋回(180°)
                 case 9:
                     counter.reset();
-                    turn_right(975);
-                    if(sum_pulse > 975) {
+                    //turn_right(975);
+                    turn_right(960);
+                    if(sum_pulse > 960) {
                         phase = 10;
                     }
                     break;
@@ -809,7 +804,7 @@
                 case 10:
                     stop();
                     counter.start();
-                    if(counter.read() > 1.0f) {
+                    if(counter.read() > 0.5f) {
                         phase = 11;
                         wheel_reset();
                     }
@@ -839,7 +834,7 @@
                 case 12:
                     stop();
                     counter.start();
-                    if(counter.read() > 1.0f) {
+                    if(counter.read() > 0.5f) {
                         phase = 13;
                         wheel_reset();
                     }
@@ -869,7 +864,7 @@
                 case 14:
                     stop();
                     counter.start();
-                    if(counter.read() > 1.0f) {
+                    if(counter.read() > 0.5f) {
                         phase = 15;
                         wheel_reset();
                     }
@@ -878,7 +873,8 @@
                 //排出
                 case 15:
                     counter.reset();
-                    tyokudo(arm_enc.getPulses(), 16);
+                    //tyokudo(arm_enc.getPulses(), 16);
+                    tyokudo(arm_enc.getPulses(), 17);
                     break;
 
                 //1秒停止
@@ -904,7 +900,7 @@
                 case 18:
                     stop();
                     counter.start();
-                    if(counter.read() > 1.0f) {
+                    if(counter.read() > 0.5f) {
                         phase = 19;
                         wheel_reset();
                     }
@@ -934,7 +930,7 @@
                 case 20:
                     stop();
                     counter.start();
-                    if(counter.read() > 1.0f) {
+                    if(counter.read() > 0.5f) {
                         phase = 21;
                         wheel_reset();
                     }
@@ -962,6 +958,7 @@
 
                 //シーツ装填
                 case 22:
+                    YELLOW_LED = 1;
                     if(start_switch == 1) {
                         wheel_reset();
                         phase = 23;
@@ -1011,9 +1008,8 @@
                 //90°左旋回
                 case 27:
                     counter.reset();
-                    //turn_left(465);
-                    turn_left(485);
-                    if(sum_pulse > 485) {
+                    turn_left(500);
+                    if(sum_pulse > 500) {
                         phase = 28;
                     }
                     break;
@@ -1061,8 +1057,8 @@
                 //掛けるところまで前進
                 case 31:
                     counter.reset();
-                    front(10000);
-                    if((y_pulse1 > 10000) || (y_pulse2 > 10000)) {
+                    front(11000);
+                    if((y_pulse1 > 11000) || (y_pulse2 > 11000)) {
                         phase = 32;
                         counter.start();
                     }
@@ -1130,7 +1126,7 @@
                     counter.start();
                     
                     //1秒間ファン送風
-                    if(counter.read() <= 2.0f) {
+                    if(counter.read() <= 1.0f) {
                         fan_data[0] = 0xFF;
                         i2c.write(0x26, fan_data, 1);
                         i2c.write(0x28, fan_data, 1);
@@ -1138,7 +1134,7 @@
                         i2c.write(0x30, servo_data, 1);
                     }
                     //1~3秒の間でサーボを開放
-                    else if((counter.read() > 2.0f) && (counter.read() <= 4.0f)) {
+                    else if((counter.read() > 1.0f) && (counter.read() <= 3.0f)) {
                         fan_data[0] = 0xFF;
                         i2c.write(0x26, fan_data, 1);
                         i2c.write(0x28, fan_data, 1);
@@ -1146,7 +1142,7 @@
                         i2c.write(0x30, servo_data, 1);
                     }
                     //3秒過ぎたら終わり
-                    else if(counter.read() > 4.0f) {
+                    else if(counter.read() > 3.0f) {
                         fan_data[0] = 0x80;
                         i2c.write(0x26, fan_data, 1);
                         i2c.write(0x28, fan_data, 1);
@@ -1176,6 +1172,8 @@
 
     start_switch.mode(PullUp);
     zone_switch.mode(PullDown);
+    
+    YELLOW_LED = 0;
 
     //非常停止関連
     pic.baud(19200);
@@ -1228,6 +1226,7 @@
 }
 
 void print_pulses(void) {
+        //pc.printf("%d\n\r", RDATA);
         //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("f: %d, b: %d, r: %d, phase: %d\n\r", front_limit, back_limit, right_limit, phase);
@@ -1246,7 +1245,7 @@
     else if(RDATA == '9'){
         myled = 0.2;
         emergency = 0;
-        
+        /*
         //終了phaseで駆動系統OFF
         if(phase >= 39) {
             emergency = 1;
@@ -1254,6 +1253,7 @@
         else if(phase < 39) {
             emergency = 0;
         }
+        */
     }
 }
 
@@ -1584,8 +1584,8 @@
             //後ろのリミットが押されたら強制停止
             if(kaisyu_usiro_limit == 1) {
                 arm_motor[0] = 0x80;
+                kaisyu_phase = 2;
                 phase = next_phase;
-                kaisyu_phase = 2;
             }
             break;
 
@@ -1671,20 +1671,31 @@
             //直動の前リミットが押されたら
             else if(tyokudo_mae_limit == 1) {
                 //高速後進
-                arm_motor[0] = 0x40;
+                arm_motor[0] = 0x4C;
                 drop_motor[0] = 0x00;
                 tyokudo_phase = 1;
             }
             break;
 
         case 1:
-            //後進->減速
-            //リミットが押されたら強制停止
+            //後進->停止
             if(tyokudo_usiro_limit == 1) {
+                drop_motor[0] = 0x80;
+                
+                if(kaisyu_mae_limit == 1) {
+                    arm_motor[0] = 0x80;
+                    tyokudo_phase = 2;
+                    phase = next_phase;
+                }
+            }
+            if(kaisyu_mae_limit == 1) {
                 arm_motor[0] = 0x80;
-                drop_motor[0] = 0x80;
-                tyokudo_phase = 2;
-                phase = next_phase;
+                
+                if(tyokudo_usiro_limit == 1) {
+                    drop_motor[0] = 0x80;
+                    tyokudo_phase = 2;
+                    phase = next_phase;
+                }
             }
             break;
             
@@ -1693,7 +1704,7 @@
             drop_motor[0] = 0x80;
             break;
     }
-    
+    //回収MD・排出MDへ書き込み
     i2c.write(0x18, arm_motor,  1);
     i2c.write(0x20, drop_motor, 1);
 }
@@ -2035,11 +2046,10 @@
         //制御量の最小、最大
         //左進(目標まで達していない)
         if((x_pulse1 < target) && (x_pulse2 < target)) {
-            left_migimae.setOutputLimits(0xEC,     0xED);   //0xFFに近いほうが速い
-            left_migiusiro.setOutputLimits(0x7A,   0x7B);   //0x7Bに近いほうが速い
-            //left_hidarimae.setOutputLimits(0x6B, 0x6C);     //0x7Bに近いほうが速い
-            left_hidarimae.setOutputLimits(0x6E, 0x6F);
-            left_hidariusiro.setOutputLimits(0xFE, 0xFF);   //0xFFに近いほうが速い
+            left_migimae.setOutputLimits(0xEC,     0xED);
+            left_migiusiro.setOutputLimits(0x7A,   0x7B);
+            left_hidarimae.setOutputLimits(0x6E,   0x6F);
+            left_hidariusiro.setOutputLimits(0xFE, 0xFF);
         }
         //停止(目標より行き過ぎ)
         else if((x_pulse1 > target) && (x_pulse2 > target)) {