Diff: main.cpp

Revision:

20:ac4954be1fe0

Parent:

19:f17d2e585973

Child:

21:89db2a19e52e

diff -r f17d2e585973 -r ac4954be1fe0 main.cpp
--- a/main.cpp	Tue Sep 17 01:33:16 2019 +0000
+++ b/main.cpp	Wed Sep 18 03:36:25 2019 +0000
@@ -105,6 +105,7 @@
 char arm_moter[1], drop_moter[1];
 char fan_data[1];
 char servo_data[1];
+char right_arm_data[1], left_arm_data[1];
 
 //非常停止関連変数
 char RDATA;
@@ -123,26 +124,32 @@
 int right_limit = 0;
 //後部が壁に当たっているか
 int back_limit = 0;
+//回収機構の下限(引っ込めてるほう)
+bool kaisyu_limit = 0;
+//右腕の下限
+bool right_arm_lower_limit = 0;
 //右腕の上限
 bool right_arm_upper_limit = 0;
-//右腕の下限
-bool right_arm_lower_limit = 0;
-//左腕の上限
-bool left_arm_upper_limit = 0;
 //左腕の下限
 bool left_arm_lower_limit = 0;
-//吐き出し機構の下限
-bool tyokudo_usiro = 0;
+//左腕の上限
+bool left_arm_upper_limit = 0;
 //吐き出し機構の上限
-bool tyokudo_mae = 0;
-//回収機構の上限
-bool kaisyu_mae = 0;
-//回収機構の下限
-bool kaisyu_usiro = 0;
+bool tyokudo_mae_limit = 0;
+//吐き出し機構の下限
+bool tyokudo_usiro_limit = 0;
+
 
-int masked_lower_front_limit_data = 0;
-int masked_lower_back_limit_data = 0;
-int masked_lower_right_limit_data = 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_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;
 
 //関数のプロトタイプ宣言
 void init(void);
@@ -155,6 +162,7 @@
 void wheel_reset(void);
 void kaisyu(int pulse);
 void tyokudo(int pulse);
+void arm_up(void);
 void front(int target);
 void back(int target);
 void right(int target);
@@ -171,30 +179,31 @@
 void dondonkasoku(void);
 
 int main(void) {
-    
+
     init();
     init_send();
-    
+
     //とりあえず(後で消してね)
     zone = BLUE;
-    
+
     while(1) {
-        
+
         get_pulses();
         print_pulses();
         get_emergency();
         read_limit();
+
+        //if(start_switch == 1) {
+            //tyokudo(arm_enc.getPulses());
+        //}
         
         //青ゾーン
         if(zone == BLUE) {
-            
+
             switch(phase) {
-                
+
                 //スタート位置へセット
                 case 0:
-                    servo_data[0] = 0x03;
-                    i2c.write(0x30, servo_data, 1);
-                    
                     //リミットが洗濯物台に触れているか
                     if(right_limit == 3) {
                         USR_LED1 = 1;
@@ -207,23 +216,32 @@
                         USR_LED1 = 0;
                     }
                     break;
-                    
+
                 //回収
                 case 1:
-                    //ここに回収動作が来るが今回は飛ばす
-                    phase = 2;
+                    //回収アームのリミットが押されているか
+                    if(kaisyu_limit == 1) {
+                        kaisyu(arm_enc.getPulses());
+                        USR_LED2 = 1;
+                    } else {
+                        USR_LED2 = 0;
+                    }
+                    servo_data[0] = 0x03;
+                    i2c.write(0x30, servo_data, 1);
                     break;
-                   
+
                 //1秒停止
                 case 2:
                     stop();
+                    servo_data[0] = 0x04;
+                    i2c.write(0x30, servo_data, 1);
                     counter.start();
                     if(counter.read() > 1.0f) {
                         phase = 3;
                         wheel_reset();
                     }
                     break;
-                     
+
                 //左移動
                 case 3:
                     counter.reset();
@@ -232,7 +250,7 @@
                         phase = 4;
                     }
                     break;
-                 
+
                 //1秒停止
                 case 4:
                     stop();
@@ -242,16 +260,16 @@
                         wheel_reset();
                     }
                     break;
-                
+
                 //右旋回(180°)
                 case 5:
                     counter.reset();
-                    turn_right(520);
-                    if(x_pulse2 > 520) {
+                    turn_right(518);
+                    if(x_pulse2 > 518) {
                         phase = 6;
                     }
                     break;
-                    
+
                 //1秒停止
                 case 6:
                     stop();
@@ -261,12 +279,12 @@
                         wheel_reset();
                     }
                     break;
-                    
+
                 //ちょっくら右移動
                 case 7:
                     counter.reset();
                     right(-1000);
-                    
+
                     if((x_pulse1*-1 > 500) && (x_pulse2*-1 > 500)) {
                         true_migimae_data[0] = 0x94;
                         true_migiusiro_data[0] = 0x10;
@@ -277,7 +295,7 @@
                         phase = 8;
                     }
                     break;
-                    
+
                 //1秒停止
                 case 8:
                     stop();
@@ -287,20 +305,21 @@
                         wheel_reset();
                     }
                     break;
-                
+
                 //排出
                 case 9:
                     counter.reset();
+                    tyokudo(arm_enc.getPulses());
                     //ここに排出動作が来るが今回は飛ばす
-                    phase = 10;
+                    //phase = 10;
                     break;
-                
+
                 //排出機構引っ込める
                 case 10:
                     //ここに排出機構引っ込める動作が来るが今回は飛ばす
                     phase = 11;
                     break;
-                    
+
                 //1秒停止
                 case 11:
                     stop();
@@ -310,7 +329,7 @@
                         wheel_reset();
                     }
                     break;
-                    
+
                 //前進
                 case 12:
                     counter.reset();
@@ -319,7 +338,7 @@
                         phase = 13;
                     }
                     break;
-                    
+
                 //1秒停止
                 case 13:
                     stop();
@@ -329,7 +348,7 @@
                         wheel_reset();
                     }
                     break;
-                    
+
                 //右移動
                 case 14:
                     counter.reset();
@@ -344,7 +363,7 @@
                         phase = 15;
                     }
                     break;
-                    
+
                 //シーツ装填
                 case 15:
                     if(start_switch == 1) {
@@ -353,7 +372,7 @@
                         stop();
                     }
                     break;
-                    
+
                 //竿のラインまで前進
                 case 16:
                     counter.reset();
@@ -362,7 +381,7 @@
                         phase = 17;
                     }
                     break;
-                    
+
                 //1秒停止
                 case 17:
                     stop();
@@ -372,8 +391,8 @@
                         wheel_reset();
                     }
                     break;
-                      
-                //掛けるところまで左移動        
+
+                //掛けるところまで左移動
                 case 18:
                     counter.reset();
                     left(10000);
@@ -381,7 +400,7 @@
                         phase = 19;
                     }
                     break;
-                    
+
                 //1秒停止
                 case 19:
                     stop();
@@ -390,17 +409,17 @@
                         phase = 20;
                         wheel_reset();
                     }
-                    break;  
-                    
+                    break;
+
                 //妨害防止の右旋回
-                case 20:   
+                case 20:
                     counter.reset();
                     turn_right(300);
                     if(x_pulse2 > 300) {
                         phase = 21;
                     }
                     break;
-                    
+
                 //1秒停止
                 case 21:
                     stop();
@@ -409,19 +428,19 @@
                         phase = 22;
                         wheel_reset();
                     }
-                    break; 
-                    
-                //カウンターリセット
+                    break;
+
+                //アームアップ
                 case 22:
                     stop();
                     counter.reset();
-                    phase = 23;
+                    arm_up();
                     break;
-                    
+                
                 //シーツを掛ける
                 case 23:
                     counter.start();
-                    
+
                     //5秒間ファン送風
                     if(counter.read() <= 4.0f) {
                         fan_data[0] = 0xFF;
@@ -445,13 +464,13 @@
                         phase = 24;
                     }
                     break;
-                    
+
                 //終了っ！(守衛さん風)
                 case 24:
                     //駆動系統OFF
                     emergency = 0;
                     break;
-                
+
                 default:
                     //駆動系統OFF
                     emergency = 0;
@@ -465,46 +484,52 @@
 
     //通信ボーレートの設定
     pc.baud(460800);
-    
+
     limit_serial.baud(115200);
-    
+
     start_switch.mode(PullUp);
-    
+
     //非常停止関連
     pic.baud(19200);
     pic.format(8, Serial::None, 1);
     pic.attach(get, Serial::RxIrq);
-    
+
     x_pulse1 = 0;   x_pulse2 = 0;   y_pulse1 = 0;   y_pulse2 = 0;
     migimae_data[0] = 0x80; migiusiro_data[0] = 0x80;   hidarimae_data[0] = 0x80;   hidariusiro_data[0] = 0x80;
     true_migimae_data[0] = 0x80;    true_migiusiro_data[0] = 0x80;  true_hidarimae_data[0] = 0x80;  true_hidariusiro_data[0] = 0x80;
     fan_data[0] = 0x80;
     servo_data[0] = 0x80;
+    right_arm_data[0] = 0x80;   left_arm_data[0] = 0x80;
 }
 
 void init_send(void) {
-    
+
     init_send_data[0] = 0x80;
     i2c.write(0x10, init_send_data, 1);
     i2c.write(0x12, init_send_data, 1);
     i2c.write(0x14, init_send_data, 1);
     i2c.write(0x16, init_send_data, 1);
+    i2c.write(0x18, init_send_data, 1);
+    i2c.write(0x20, init_send_data, 1);
+    i2c.write(0x22, init_send_data, 1);
+    i2c.write(0x24, init_send_data, 1);
+    i2c.write(0x30, init_send_data, 1);
     wait(0.1);
 }
 
 void get(void) {
-    
-    baff = pic.getc();    
-    
+
+    baff = pic.getc();
+
     for(; flug; flug--)
         RDATA = baff;
-        
+
     if(baff == ':')
         flug = 1;
 }
 
 void get_pulses(void) {
-        
+
         x_pulse1 = wheel_x1.getPulses();
         x_pulse2 = wheel_x2.getPulses();
         y_pulse1 = wheel_y1.getPulses();
@@ -513,14 +538,15 @@
 
 void print_pulses(void) {
         
+        pc.printf("%d\n\r", tyokudo_phase);
+        //pc.printf("%d\n\r", arm_enc.getPulses());
         //pc.printf("limit: 0x%x, upper: 0x%x, lower: 0x%x\n\r", limit_data, upper_limit_data, lower_limit_data);
-        pc.printf("upper: 0x%x, low: 0x%x, f: %d, b: %d, r: %d\n\r", upper_limit_data, lower_limit_data, front_limit, back_limit, right_limit);
         //pc.printf("x1: %d, x2: %d, y1: %d, y2: %d, phase: %d\n\r", x_pulse1, x_pulse2, y_pulse1, y_pulse2, phase);
         //pc.printf("RF: %x, RB: %x, LF: %x, LB: %x\n\r", true_migimae_data[0], true_migiusiro_data[0], true_hidarimae_data[0], true_hidariusiro_data[0]);
 }
 
 void get_emergency(void) {
-    
+
     if(RDATA == '1') {
         myled = 1;
         emergency = 1;
@@ -532,24 +558,32 @@
 }
 
 void read_limit(void) {
-        
+
         limit_data = limit_serial.getc();
-        
+
         //上位1bitが1ならば下のリミットのデータだと判断
         if((limit_data & 0b10000000) == 0b10000000) {
             lower_limit_data = limit_data;
-            //upper_limit_data = 0b01111111;
-            
+
         //上位1bitが0ならば上のリミットのデータだと判断
         } else {
             upper_limit_data = limit_data;
-            //lower_limit_data = 0b11111111;
         }
-        
+
+        //下リミット基板からのデータのマスク処理
         masked_lower_front_limit_data = lower_limit_data & 0b00000011;
         masked_lower_back_limit_data  = lower_limit_data & 0b00001100;
         masked_lower_right_limit_data = lower_limit_data & 0b00110000;
-        
+        masked_kaisyu_limit_data      = lower_limit_data & 0b01000000;
+
+        //上リミット基板からのデータのマスク処理
+        masked_right_arm_lower_limit_data = upper_limit_data & 0b00000001;
+        masked_right_arm_upper_limit_data = upper_limit_data & 0b00000010;
+        masked_left_arm_lower_limit_data  = upper_limit_data & 0b00000100;
+        masked_left_arm_upper_limit_data  = upper_limit_data & 0b00001000;
+        masked_tyokudo_mae_limit_data     = upper_limit_data & 0b00010000;
+        masked_tyokudo_usiro_limit_data   = upper_limit_data & 0b00100000;
+
         //前部リミット
         switch(masked_lower_front_limit_data) {
             //両方押された
@@ -568,7 +602,7 @@
                 front_limit = 0;
                 break;
         }
-        
+
         //後部リミット
         switch(masked_lower_back_limit_data) {
             //両方押された
@@ -587,7 +621,7 @@
                 back_limit = 0;
                 break;
         }
-        
+
         //右部リミット
         switch(masked_lower_right_limit_data) {
             //両方押された
@@ -606,68 +640,108 @@
                 right_limit = 0;
                 break;
         }
-        
-        /*
-        //回収機構の上限
-        if((lower_limit_data & 0b01000000) == 0b01000000) {
-            withdrawal_upper_limit = 1;
-        } else {
-            withdrawal_upper_limit = 0;
+
+        //回収機構リミット
+        switch(masked_kaisyu_limit_data) {
+            //押された
+            case 0b00000000:
+                kaisyu_limit = 1;
+                break;
+            case 0b01000000:
+                  kaisyu_limit = 0;
+                break;
+            default:
+                kaisyu_limit = 0;
+                break;
         }
-        
-        //回収機構の下限
-        if((lower_limit_data & 0b10000000) == 0b10000000) {
-            withdrawal_lower_limit = 1;
-        } else {
-            withdrawal_lower_limit = 0;
+
+        //右腕下部リミット
+        switch(masked_right_arm_lower_limit_data) {
+            //押された
+            case 0b00000000:
+                right_arm_lower_limit = 1;
+                break;
+            case 0b00000001:
+                right_arm_lower_limit = 0;
+                break;
+            default:
+                right_arm_lower_limit = 0;
+                break;
         }
-            
-        //右腕の上限
-        if((upper_limit_data & 0b00000001) == 0b00000001) {
-            right_arm_upper_limit = 1;
-        } else {
-            right_arm_upper_limit = 0;
-        }
-        
-        //右腕の下限
-        if((upper_limit_data & 0b00000010) == 0b00000010) {
-            right_arm_lower_limit = 1;
-        } else {
-            right_arm_lower_limit = 0;
+
+        //右腕上部リミット
+        switch(masked_right_arm_upper_limit_data) {
+            //押された
+            case 0b00000000:
+                right_arm_upper_limit = 1;
+                break;
+            case 0b00000010:
+                right_arm_upper_limit = 0;
+                break;
+            default:
+                right_arm_upper_limit = 0;
+                break;
         }
-        
-        //左腕の上限
-        if((upper_limit_data & 0b00000100) == 0b00000100) {
-            left_arm_upper_limit = 1;
-        } else {
-            left_arm_upper_limit = 0;
+
+        //左腕下部リミット
+        switch(masked_left_arm_lower_limit_data) {
+            //押された
+            case 0b00000000:
+                left_arm_lower_limit = 1;
+                break;
+            case 0b00000100:
+                left_arm_lower_limit = 0;
+                break;
+            default:
+                left_arm_lower_limit = 0;
+                break;
         }
-        
-        //左腕の下限
-        if((upper_limit_data & 0b00001000) == 0b00001000) {
-            left_arm_lower_limit = 1;
-        } else {
-            left_arm_lower_limit = 0;
+
+        //左腕上部リミット
+        switch(masked_left_arm_upper_limit_data) {
+            //押された
+            case 0b00000000:
+                left_arm_upper_limit = 1;
+                break;
+            case 0b00001000:
+                left_arm_upper_limit = 0;
+                break;
+            default:
+                left_arm_upper_limit = 0;
+                break;
         }
-        
-        //吐き出し機構の上限
-        if((upper_limit_data & 0b00010000) == 0b00010000) {
-            force_out_upper_limit = 1;
-        } else {
-            force_out_upper_limit = 0;
+
+        //直動の前
+        switch(masked_tyokudo_mae_limit_data) {
+            //押された
+            case 0b00000000:
+                tyokudo_mae_limit = 1;
+                break;
+            case 0b00010000:
+                tyokudo_mae_limit = 0;
+                break;
+            default:
+                tyokudo_mae_limit = 0;
+                break;
         }
-        
-        //吐き出し機構の下限
-        if((upper_limit_data & 0b00100000) == 0b00100000) {
-            force_out_lower_limit = 1;
-        } else {
-            force_out_lower_limit = 0;
+
+        //直動の後
+        switch(masked_tyokudo_usiro_limit_data) {
+            //押された
+            case 0b00000000:
+                tyokudo_usiro_limit = 1;
+                break;
+            case 0b00100000:
+                tyokudo_usiro_limit = 0;
+                break;
+            default:
+                tyokudo_usiro_limit = 0;
+                break;
         }
-        */
 }
 
 void wheel_reset(void) {
-    
+
     wheel_x1.reset();
     wheel_x2.reset();
     wheel_y1.reset();
@@ -677,104 +751,186 @@
 void kaisyu(int pulse) {
 
     switch (kaisyu_phase) {
+        
         case 0:
-            arm_moter[0] = 0x80;
-            kaisyu_phase = 0;
-            break;
-        case 1:
             //前進->減速
-            if(pulse < 2000) {
+            //3000pulseまで高速前進
+            if(pulse < 3000) {
                 arm_moter[0] = 0xFF;
+                //kaisyu_phase = 1;
+            } 
+            
+            //3000pulse超えたら低速前進
+            else if(pulse >= 3000) {
+                arm_moter[0] = 0xB3;
                 kaisyu_phase = 1;
-            } else {
-                arm_moter[0] = 0xB3;
-                kaisyu_phase = 2;
             }
             break;
-        case 2:
+            
+        case 1:
             //前進->停止->後進
-            if(kaisyu_usiro == 1) {
+            //3600pulseまで低速前進
+            if(pulse < 3600) {
                 arm_moter[0] = 0xB3;
-                kaisyu_phase = 2;
-            } else {
-                arm_moter[0] =  0x80;
+                //kaisyu_phase = 2;
+            } 
+            
+            //3600pulse超えたら停止
+            else if(pulse >= 3600) {
+                arm_moter[0] = 0x80;
+                
+                //1秒待ってから引っ込める
+                counter.start();
+                if(counter.read() > 1.0f) {
+                    kaisyu_phase = 2;
+                }
+            }
+                        
+            break;
+            
+        case 2:
+            //後進->減速
+            //500pulseまで高速後進
+            counter.reset();
+            if(pulse > 500) {
+                arm_moter[0] = 0x00;
+                //kaisyu_phase = 3;
+                
+            } 
+            //500pulse以下になったら低速後進
+            else if(pulse <= 500) {
+                arm_moter[0] = 0x4C;
                 kaisyu_phase = 3;
-                i2c.write(0x18, arm_moter,1);
-                wait(1);
             }
             break;
+            
         case 3:
-            //後進->減速
-            if(pulse > 1600) {
-                arm_moter[0] = 0x00;
-                kaisyu_phase = 3;
-            } else {
+            //後進->停止
+            //リミット押されるまで低速後進
+            if(pulse <= 500) {
                 arm_moter[0] = 0x4C;
+                //kaisyu_phase = 4;
+            }
+            
+            //リミット押されたら停止
+            if(kaisyu_limit == 1) {
+                arm_moter[0] = 0x80;
                 kaisyu_phase = 4;
+                phase = 2;   
             }
             break;
-        case 4:
-            //後進->停止
-            if(kaisyu_mae == 1) {
-                arm_moter[0] = 0x4C;
-                kaisyu_phase = 4;
-            } else {
-                arm_moter[0] = 0x80;
-                kaisyu_phase = 0;
-                phase = 1;
-            }
-            break;
+            
         default:
+            arm_moter[0] = 0x80;
             break;
     }
     
+    //回収MDへ書き込み
     i2c.write(0x18, arm_moter, 1);
 }
 
 void tyokudo(int pulse) {
-    
-    switch (tyokudo_phase) {
+
+    switch(tyokudo_phase) {
+        
         case 0:
-            arm_moter[0] = 0x80;
-            drop_moter[0] = 0x80;
-            break;
-        case 1:
             //前進->減速
-            arm_moter[0] = (pulse < 2000)? 0xCD:0xC0;
-            drop_moter[0] = (pulse < 2000)? 0xE6:0xCD;
-            tyokudo_phase = (pulse < 2000)? 1:2;
-            break;
-        case 2:
-            //前進(遅い)->停止->後進(早い)
-            if(tyokudo_mae == 1) {
-                arm_moter[0] = 0xC0;
-                drop_moter[0] = 0xCD;
-                tyokudo_phase = 2;
-            } else {
-                arm_moter[0] = 0x80;
+            
+            //3600pulseより大きい&直堂前リミットが押されたら次のphaseへ移行
+            if(tyokudo_mae_limit == 0) {
+                //2000pulseまで高速前進
+                if(pulse < 2000) {
+                    arm_moter[0] = 0xCD;
+                    drop_moter[0] = 0xE6;
+                } 
+                //2000pulse以上で低速前進
+                else if(pulse >= 2000) {
+                    arm_moter[0] = 0xC0;
+                    drop_moter[0] = 0xCD;
+                }
+                //パルスが3600を終えたらアームのみ強制停止
+                else if(pulse > 3600) {
+                    arm_moter[0] = 0x80;
+                    drop_moter[0] = 0xCD;
+                }
+            }
+            
+            //直動の前リミットが押されたら
+            else if(tyokudo_mae_limit == 1) {
+                //直動のみ強制停止
                 drop_moter[0] = 0x80;
-                tyokudo_phase = 3;
-                i2c.write(0x18, arm_moter,1);
-                i2c.write(0x20, drop_moter,1);
-                wait(1);
+                
+                //2000pulseまで高速前進
+                if(pulse < 2000) {
+                    arm_moter[0] = 0xCD;
+                }
+                //2000pulse以上で低速前進
+                else if(pulse >= 2000) {
+                    arm_moter[0] = 0xC0;
+                }
+                //パルスが3600を終えたらアームも強制停止
+                else if(pulse > 3600) {
+                    arm_moter[0] = 0x80;
+                    tyokudo_phase = 1;
+                }
             }
             break;
-        case 3:
+            
+        case 1:
             //後進->減速
-            arm_moter[0] = (pulse > 1600)? 0x33:0x33;
-            drop_moter[0] = (pulse > 1600)? 0x19:0x19;
-            tyokudo_phase = (pulse > 1600)? 3:4;
+ 
+            //1600pulseより大きいとき高速後進
+            if(pulse > 1600) {
+                
+                if(kaisyu_limit == 0) {
+                    arm_moter[0] = 0x00;
+                }
+                //リミットが押されたら強制停止
+                else if(kaisyu_limit == 1) {
+                    arm_moter[0] = 0x80;
+                }
+                
+                if(tyokudo_usiro_limit == 0) {
+                    drop_moter[0] = 0x19;      
+                }
+                //リミットが押されたら強制停止
+                else if(tyokudo_usiro_limit == 1) {
+                    drop_moter[0] = 0x80;
+                }
+            }
+            //500pulse以下でphase2へ移行
+            else if(pulse <= 500) {
+                tyokudo_phase = 2;
+            }
             break;
-        case 4:
+            
+        case 2:
             //後進->停止
-            arm_moter[0] = (tyokudo_usiro == 1)? 0x33:0x80;
-            drop_moter[0] = (tyokudo_usiro == 1)? 0x19:0x80;
-            tyokudo_phase = (tyokudo_usiro == 1)? 4:0;
+            
+            //直動後リミットが押されるまで後進
+            if(tyokudo_usiro_limit == 0) {
+                drop_moter[0] = 0x19;
+            } 
+            //直動後リミットが押されたら停止
+            else if(tyokudo_usiro_limit == 1) {
+                drop_moter[0] = 0x80;
+            }
+            
+            //リミット押されるまで低速後進
+            if(kaisyu_limit == 0) {
+                arm_moter[0] = 0x4C;
+            }
+            else if(kaisyu_limit == 1) {
+                arm_moter[1] = 0x80;
+                tyokudo_phase = 3;
+                phase = 10;
+            }
+            
             break;
+            
         default:
             arm_moter[0] = 0x80;
             drop_moter[0] = 0x80;
-            tyokudo_phase = 0;
             break;
     }
     
@@ -782,8 +938,32 @@
     i2c.write(0x20, drop_moter, 1);
 }
 
+void arm_up(void) {
+    
+    //両腕、上限リミットが押されてなかったら上昇
+    if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 0)) {
+        right_arm_data[0] = 0xFF;   left_arm_data[0] = 0xFF;
+    }
+    //右腕のみリミットが押されたら左腕のみ上昇
+    else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 0)) {
+        right_arm_data[0] = 0x80;   left_arm_data[0] = 0xFF;
+    }
+    //左腕のみリミットが押されたら右腕のみ上昇
+    else if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 1)) {
+        right_arm_data[0] = 0xFF;   left_arm_data[0] = 0x80;
+    }
+    //両腕、上限リミットが押されたら停止
+    else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 1)) {
+        right_arm_data[0] = 0x80;   left_arm_data[0] = 0x80;
+        phase = 23;
+    }
+    
+    i2c.write(0x22, right_arm_data, 1);
+    i2c.write(0x24, left_arm_data, 1);
+}
+
 void front(int target) {
-        
+
         front_PID(target);
         i2c.write(0x10, true_migimae_data,     1, false);
         i2c.write(0x12, true_migiusiro_data,   1, false);
@@ -793,7 +973,7 @@
 }
 
 void back(int target) {
-    
+
         back_PID(target);
         i2c.write(0x10, true_migimae_data,     1, false);
         i2c.write(0x12, true_migiusiro_data,   1, false);
@@ -803,7 +983,7 @@
 }
 
 void right(int target) {
-    
+
         right_PID(target);
         i2c.write(0x10, true_migimae_data,     1, false);
         i2c.write(0x12, true_migiusiro_data,   1, false);
@@ -813,7 +993,7 @@
 }
 
 void left(int target) {
-    
+
         left_PID(target);
         i2c.write(0x10, true_migimae_data,     1, false);
         i2c.write(0x12, true_migiusiro_data,   1, false);
@@ -823,7 +1003,7 @@
 }
 
 void turn_right(int target) {
-    
+
         turn_right_PID(target);
         i2c.write(0x10, true_migimae_data,     1, false);
         i2c.write(0x12, true_migiusiro_data,   1, false);
@@ -833,7 +1013,7 @@
 }
 
 void turn_left(int target) {
-    
+
         turn_left_PID(target);
         i2c.write(0x10, true_migimae_data,     1, false);
         i2c.write(0x12, true_migiusiro_data,   1, false);
@@ -843,12 +1023,12 @@
 }
 
 void stop(void) {
-        
+
         true_migimae_data[0]     = 0x80;
         true_migiusiro_data[0]   = 0x80;
         true_hidarimae_data[0]   = 0x80;
-        true_hidariusiro_data[0] = 0x80;      
-          
+        true_hidariusiro_data[0] = 0x80;
+
         i2c.write(0x10, true_migimae_data,     1, false);
         i2c.write(0x12, true_migiusiro_data,   1, false);
         i2c.write(0x14, true_hidarimae_data,   1, false);
@@ -952,7 +1132,7 @@
 }
 
 void back_PID(int target) {
-    
+
         //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
         back_migimae.setInputLimits(-2147483648,     2147483647);
         back_migiusiro.setInputLimits(-2147483648,   2147483647);
@@ -1143,7 +1323,7 @@
 }
 
 void left_PID(int target) {
-    
+
         //センサ出力値の最小、最大
         left_migimae.setInputLimits(-2147483648,     2147483647);
         left_migiusiro.setInputLimits(-2147483648,   2147483647);
@@ -1179,7 +1359,7 @@
             left_hidarimae.setOutputLimits(0x7C,   0x83);
             left_hidariusiro.setOutputLimits(0x7C, 0x83);
         }
-        
+
         //よくわからんやつ
         left_migimae.setMode(AUTO_MODE);
         left_migiusiro.setMode(AUTO_MODE);
@@ -1301,7 +1481,7 @@
 }
 
 void turn_left_PID(int target) {
-            
+
         //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
         turn_left_migimae.setInputLimits(-2147483648,     2147483647);
         turn_left_migiusiro.setInputLimits(-2147483648,   2147483647);