ウオッチドッグタイマを追加したよん

Dependencies:   mbed QEI PID

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 /* -------------------------------------------------------------------  */
00002 /* NHK ROBOCON 2019 Ibaraki Kosen A team Automatic                      */
00003 /* Nucleo Type: F446RE                                                  */
00004 /* designed by Yuhi Takaku from 5D, mail: rab1sy23@gmail.com            */
00005 /* Actuator: RS-555*4, RS-385*4, RZ-735*2, PWM_Servo(KONDO)*2           */
00006 /* Sensor: encorder*4, limit_switch*14                                  */
00007 /* -------------------------------------------------------------------  */
00008 /* Both of areas are compleated!                                        */
00009 /* -------------------------------------------------------------------  */
00010 #include "mbed.h"
00011 #include "math.h"
00012 #include "QEI.h"
00013 #include "PID.h"
00014 
00015 //終了phase
00016 #define FINAL_PHASE 50
00017 
00018 #define RED     0
00019 #define BLUE    1
00020 
00021 //#define wind_time1  1.00f
00022 //#define wind_time2  1.00f
00023 
00024 #define wind_time1  0.75f
00025 //#define wind_time2  1.50f
00026 #define wind_time2  1.75f
00027 
00028 //PID Gain of wheels(Kp, Ti, Td, control cycle)
00029 //前進
00030 PID front_migimae(4500000.0, 0.0, 0.0, 0.001);
00031 PID front_migiusiro(4500000.0, 0.0, 0.0, 0.001);
00032 PID front_hidarimae(4500000.0, 0.0, 0.0, 0.001);
00033 PID front_hidariusiro(4500000.0, 0.0, 0.0, 0.001);
00034 
00035 //後進
00036 PID back_migimae(4500000.0, 0.0, 0.0, 0.001);
00037 PID back_migiusiro(4500000.0, 0.0, 0.0, 0.001);
00038 PID back_hidarimae(4500000.0, 0.0, 0.0, 0.001);
00039 PID back_hidariusiro(4500000.0, 0.0, 0.0, 0.001);
00040 
00041 //右進
00042 PID right_migimae(6000000.0, 0.0, 0.0, 0.001);
00043 PID right_migiusiro(6000000.0, 0.0, 0.0, 0.001);
00044 PID right_hidarimae(6000000.0, 0.0, 0.0, 0.001);
00045 PID right_hidariusiro(6000000.0, 0.0, 0.0, 0.001);
00046 
00047 //左進
00048 PID left_migimae(6000000.0, 0.0, 0.0, 0.001);
00049 PID left_migiusiro(6000000.0, 0.0, 0.0, 0.001);
00050 PID left_hidarimae(6000000.0, 0.0, 0.0, 0.001);
00051 PID left_hidariusiro(6000000.0, 0.0, 0.0, 0.001);
00052 
00053 //右旋回
00054 PID turn_right_migimae(3000000.0, 0.0, 0.0, 0.001);
00055 PID turn_right_migiusiro(3000000.0, 0.0, 0.0, 0.001);
00056 PID turn_right_hidarimae(3000000.0, 0.0, 0.0, 0.001);
00057 PID turn_right_hidariusiro(3000000.0, 0.0, 0.0, 0.001);
00058 
00059 //左旋回
00060 PID turn_left_migimae(3000000.0, 0.0, 0.0, 0.001);
00061 PID turn_left_migiusiro(3000000.0, 0.0, 0.0, 0.001);
00062 PID turn_left_hidarimae(3000000.0, 0.0, 0.0, 0.001);
00063 PID turn_left_hidariusiro(3000000.0, 0.0, 0.0, 0.001);
00064 
00065 //MDとの通信ポート
00066 I2C i2c(PB_9, PB_8);  //SDA, SCL
00067 
00068 //PCとの通信ポート
00069 Serial pc(USBTX, USBRX);    //TX, RX
00070 
00071 //特小モジュールとの通信ポート
00072 Serial pic(A0, A1);
00073 
00074 //リミットスイッチ基板との通信ポート
00075 Serial limit_serial(PC_12, PD_2);
00076 
00077 //12V停止信号ピン
00078 DigitalOut emergency(D11);
00079 
00080 DigitalOut USR_LED1(PB_7);
00081 //DigitalOut USR_LED2(PC_13);
00082 DigitalOut USR_LED3(PC_2);
00083 DigitalOut USR_LED4(PC_3);
00084 DigitalOut  GREEN_LED(D8);
00085 DigitalOut  RED_LED(D10);
00086 DigitalOut  YELLOW_LED(D9);
00087 
00088 //遠隔非常停止ユニットLED
00089 AnalogOut myled(A2);
00090 
00091 DigitalIn start_switch(PB_12);
00092 DigitalIn USR_SWITCH(PC_13);
00093 DigitalIn zone_switch(PC_10);
00094 
00095 QEI wheel_x1(PA_8 , PA_6 , NC, 624);
00096 QEI wheel_x2(PB_14, PB_13, NC, 624);
00097 QEI wheel_y1(PB_1 , PB_15, NC, 624);
00098 QEI wheel_y2(PA_12, PA_11, NC, 624);
00099 QEI arm_enc(PB_4,   PB_5 , NC, 624);
00100 
00101 //移動後n秒停止タイマー
00102 Timer counter;
00103 //Watch Dog Timer
00104 Timer WDT;
00105 
00106 //エンコーダ値格納変数
00107 int x_pulse1, x_pulse2, y_pulse1, y_pulse2, sum_pulse, arm_pulse;
00108 
00109 //操作の段階変数
00110 unsigned int phase = 0;
00111 unsigned int kaisyu_phase = 0;
00112 unsigned int tyokudo_phase = 0;
00113 unsigned int start_zone = 1;
00114 bool zone = RED;
00115 
00116 bool stop_flag[30] = {0};
00117 
00118 //i2c送信データ変数
00119 char init_send_data[1];
00120 char migimae_data[1], migiusiro_data[1], hidarimae_data[1], hidariusiro_data[1];
00121 char true_migimae_data[1], true_migiusiro_data[1], true_hidarimae_data[1], true_hidariusiro_data[1];
00122 char arm_motor[1], drop_motor[1];
00123 char fan_data[1];
00124 char servo_data[1];
00125 char right_arm_data[1], left_arm_data[1];
00126 
00127 //非常停止関連変数
00128 char RDATA;
00129 char baff;
00130 int flug = 0;
00131 
00132 //リミット基板からの受信データ
00133 int limit_data = 0;
00134 int upper_limit_data = 0;
00135 int lower_limit_data = 0;
00136 
00137 //各辺のスイッチが押されたかのフラグ
00138 //前部が壁に当たっているか
00139 int front_limit = 0;
00140 //右部が壁にあたあっているか
00141 int right_limit = 0;
00142 //後部が壁に当たっているか
00143 int back_limit = 0;
00144 //回収機構の下限(引っ込めてるほう)
00145 bool kaisyu_mae_limit = 0;
00146 
00147 bool kaisyu_usiro_limit = 0;
00148 
00149 //右腕の下限
00150 bool right_arm_lower_limit = 0;
00151 //右腕の上限
00152 bool right_arm_upper_limit = 0;
00153 //左腕の下限
00154 bool left_arm_lower_limit = 0;
00155 //左腕の上限
00156 bool left_arm_upper_limit = 0;
00157 //吐き出し機構の上限
00158 bool tyokudo_mae_limit = 0;
00159 //吐き出し機構の下限
00160 bool tyokudo_usiro_limit = 0;
00161 
00162 int masked_lower_front_limit_data     = 0b11111111;
00163 int masked_lower_back_limit_data      = 0b11111111;
00164 int masked_lower_right_limit_data     = 0b11111111;
00165 int masked_kaisyu_mae_limit_data      = 0b11111111;
00166 int masked_kaisyu_usiro_limit_data    = 0b11111111;
00167 int masked_right_arm_lower_limit_data = 0b11111111;
00168 int masked_right_arm_upper_limit_data = 0b11111111;
00169 int masked_left_arm_lower_limit_data  = 0b11111111;
00170 int masked_left_arm_upper_limit_data  = 0b11111111;
00171 int masked_tyokudo_mae_limit_data     = 0b11111111;
00172 int masked_tyokudo_usiro_limit_data   = 0b11111111;
00173 
00174 //関数のプロトタイプ宣言
00175 void red_move(void);
00176 void blue_move(void);
00177 void init(void);
00178 void init_send(void);
00179 void get(void);
00180 void get_pulses(void);
00181 void print_pulses(void);
00182 void get_emergency(void);
00183 void read_limit(void);
00184 void wheel_reset(void);
00185 void kaisyu(int pulse, int next_phase);
00186 void kaisyu_nobasu(int next_phase);
00187 void kaisyu_hiku(int next_phase);
00188 void tyokudo(int pulse, int next_phase);
00189 void tyokudo_nobasu(int next_phase);
00190 void tyokudo_hiku(int next_phase);
00191 void arm_up(int next_phase);
00192 void fan_on(float first_wind_time, float second_wind_time, int next_phase);
00193 void front(int target);
00194 void back(int target);
00195 void right(int target);
00196 void left(int target);
00197 void turn_right(int target);
00198 void turn_left(int target);
00199 void stop(void);
00200 void all_stop(void);
00201 void front_PID(int target);
00202 void back_PID(int target);
00203 void right_PID(int target);
00204 void left_PID(int target);
00205 void turn_right_PID(int target);
00206 void turn_left_PID(int target);
00207 
00208 int main(void) {
00209 
00210     init();
00211     init_send();
00212     //消し忘れ注意!!
00213     //phase = 50;
00214     
00215     //起動時にゾーンを読んでからループに入る(試合中誤ってスイッチ押すのを防止)
00216     while(1) {
00217         if(zone_switch == 0) {
00218             zone = BLUE;
00219         } else {
00220             zone = RED;
00221         }
00222         break;
00223     }
00224         
00225     while(1) {
00226         
00227         get_pulses();
00228         //print_pulses();
00229         get_emergency();
00230         read_limit();
00231         
00232         //move_servo_with_using_onboard-switch
00233         if(USR_SWITCH == 0) {
00234             servo_data[0] = 0x03;
00235             i2c.write(0x30, servo_data, 1);
00236         } else {
00237             servo_data[0] = 0x04;
00238             i2c.write(0x30, servo_data, 1);
00239         }
00240         /*
00241         //消し忘れ注意!!
00242         if(start_switch == 1) {
00243             counter.reset();
00244             if(zone == RED) {
00245                 phase = 35;
00246             }
00247             else if(zone == BLUE) {
00248                 phase = 38;
00249             }
00250         }
00251         */
00252         //青ゾーン
00253         if(zone == BLUE) {
00254             GREEN_LED = 1;
00255             RED_LED = 0;
00256             blue_move();
00257         }
00258         //REDゾーン
00259         else if(zone == RED) {
00260             GREEN_LED = 0;
00261             RED_LED = 1;
00262             red_move();
00263         }
00264     }
00265 }
00266 
00267 void red_move(void) {
00268     switch(phase) {
00269         
00270         //スタート位置へセット
00271         case 0:
00272             //スタートスイッチが押されたか
00273             if(start_switch == 1) {
00274                 wheel_reset();
00275                 phase = 1;
00276             }
00277             
00278             //リミットが洗濯物台に触れているか
00279             if(right_limit == 3) {
00280                 USR_LED1 = 1;
00281             } else {
00282                 USR_LED1 = 0;
00283             }
00284             break;
00285         
00286         //回収アームを伸ばす
00287         case 1:
00288             kaisyu_nobasu(2);
00289             //サーボを開いておく
00290             servo_data[0] = 0x03;
00291             i2c.write(0x30, servo_data, 1);
00292             break;
00293         
00294         //1.0秒停止
00295         case 2:
00296             if(stop_flag[0] == 0) {
00297                 stop();
00298                 servo_data[0] = 0x04;
00299                 i2c.write(0x30, servo_data, 1);
00300                 counter.stop();
00301                 counter.reset();
00302                 counter.start();
00303                 stop_flag[0] = 1;
00304             }
00305             if(counter.read() > 1.0f) {
00306                 phase = 3;
00307                 wheel_reset();
00308             }
00309             break;
00310         
00311         //ちょっと前進
00312         case 3:
00313             front(800);
00314             if((y_pulse1 > 800) || (y_pulse2 > 800)) {
00315                 phase = 4;
00316             }
00317             break;
00318 
00319         //0.5秒停止
00320         case 4:
00321             WDT.start();
00322             if(stop_flag[1] == 0) {
00323                 stop();
00324                 counter.stop();
00325                 counter.reset();
00326                 counter.start();
00327                 stop_flag[1] = 1;
00328             }
00329             if(counter.read() > 0.5f) {
00330                 phase = 5;
00331                 wheel_reset();
00332             }
00333             //WDTで3秒経過したら強制的にphase5に移行
00334             if(WDT.read() > 3.0f) {
00335                 phase = 5;
00336                 wheel_reset();
00337             }
00338             break;
00339 
00340         //回収アーム引っ込める
00341         case 5:
00342             USR_LED3 = 1;
00343             kaisyu_hiku(6);
00344             break;
00345 
00346         //左移動
00347         case 6:
00348             USR_LED4 = 1;
00349             left(11500);
00350             if((x_pulse1 > 11500) || (x_pulse2 > 11500)) {
00351                 phase = 7;
00352             }
00353             break;
00354 
00355         //1秒停止
00356         case 7:
00357             if(stop_flag[2] == 0) {
00358                 stop();
00359                 counter.stop();
00360                 counter.reset();
00361                 counter.start();
00362                 stop_flag[2] = 1;
00363             }
00364             if(counter.read() > 1.0f) {
00365                 phase = 8;
00366                 wheel_reset();
00367             }
00368             break;
00369 
00370         //右旋回(180°)
00371         case 8:
00372             turn_right(940);
00373             if(sum_pulse > 940) {
00374                 phase = 9;
00375             }
00376             break;
00377 
00378         //0.5秒停止
00379         case 9:
00380             if(stop_flag[3] == 0) {
00381                 stop();
00382                 counter.stop();
00383                 counter.reset();
00384                 counter.start();
00385                 stop_flag[3] = 1;
00386             }
00387             if(counter.read() > 0.5f) {
00388                 phase = 10;
00389                 wheel_reset();
00390             }
00391             break;
00392 
00393         //壁に当たるまで前進
00394         case 10:
00395             if(front_limit == 3) {
00396                 phase = 11;
00397             }
00398             else if(front_limit != 3){
00399                 true_migimae_data[0]     = 0xA0;
00400                 true_migiusiro_data[0]   = 0xA0;
00401                 true_hidarimae_data[0]   = 0xA0;
00402                 true_hidariusiro_data[0] = 0xA0;
00403                 i2c.write(0x10, true_migimae_data,     1, false);
00404                 i2c.write(0x12, true_migiusiro_data,   1, false);
00405                 i2c.write(0x14, true_hidarimae_data,   1, false);
00406                 i2c.write(0x16, true_hidariusiro_data, 1, false);
00407                 wait_us(20);
00408             }
00409             break;
00410 
00411         //0.5秒停止
00412         case 11:
00413             if(stop_flag[4] == 0) {
00414                 stop();
00415                 counter.stop();
00416                 counter.reset();
00417                 counter.start();
00418                 stop_flag[4] = 1;
00419             }
00420             if(counter.read() > 0.5f) {
00421                 phase = 12;
00422                 wheel_reset();
00423             }
00424             break;
00425 
00426         //壁に当たるまで右移動
00427         case 12:
00428             if(right_limit == 3) {
00429                 phase = 13;
00430             }
00431             else if(right_limit != 3) {
00432                 true_migimae_data[0]     = 0x40;
00433                 true_migiusiro_data[0]   = 0xBF;
00434                 true_hidarimae_data[0]   = 0xBF;
00435                 true_hidariusiro_data[0] = 0x40;
00436                 i2c.write(0x10, true_migimae_data,     1, false);
00437                 i2c.write(0x12, true_migiusiro_data,   1, false);
00438                 i2c.write(0x14, true_hidarimae_data,   1, false);
00439                 i2c.write(0x16, true_hidariusiro_data, 1, false);
00440                 wait_us(20);
00441             }
00442             break;
00443 
00444         //0.5秒停止
00445         case 13:
00446             if(stop_flag[5] == 0) {
00447                 stop();
00448                 counter.stop();
00449                 counter.reset();
00450                 counter.start();
00451                 stop_flag[5] = 1;
00452             }
00453             if(counter.read() > 0.5f) {
00454                 phase = 14;
00455                 wheel_reset();
00456             }
00457             break;
00458 
00459         //排出
00460         case 14:
00461             tyokudo_nobasu(15);
00462             break;
00463 
00464         //後進
00465         case 15:
00466             back(-5000);
00467             if((y_pulse1*-1 > 5000) || (y_pulse2*-1 > 5000)) {
00468                 phase = 16;
00469             }
00470             break;
00471 
00472         //0.5秒停止
00473         case 16:
00474             if(stop_flag[6] == 0) {
00475                 stop();
00476                 counter.stop();
00477                 counter.reset();
00478                 counter.start();
00479                 stop_flag[6] = 1;
00480             }
00481             if(counter.read() > 0.5f) {
00482                 phase = 17;
00483                 wheel_reset();
00484             }
00485             break;
00486 
00487         //排出しまう
00488         case 17:
00489             tyokudo_hiku(18);
00490             break;
00491 
00492         //0.5秒停止
00493         case 18:
00494             if(stop_flag[7] == 0) {
00495                 stop();
00496                 counter.stop();
00497                 counter.reset();
00498                 counter.start();
00499                 stop_flag[7] = 1;
00500             }
00501             if(counter.read() > 0.5f) {
00502                 phase = 19;
00503                 wheel_reset();
00504             }
00505             break;
00506 
00507         //壁に当たるまで右移動
00508         case 19:
00509             if(right_limit == 3) {
00510                 phase = 20;
00511             }
00512             else if(right_limit != 3) {
00513                 true_migimae_data[0]     = 0x40;
00514                 true_migiusiro_data[0]   = 0xBF;
00515                 true_hidarimae_data[0]   = 0xBF;
00516                 true_hidariusiro_data[0] = 0x40;
00517                 i2c.write(0x10, true_migimae_data,     1, false);
00518                 i2c.write(0x12, true_migiusiro_data,   1, false);
00519                 i2c.write(0x14, true_hidarimae_data,   1, false);
00520                 i2c.write(0x16, true_hidariusiro_data, 1, false);
00521                 wait_us(20);
00522             }
00523             break;
00524 
00525         //0.5秒停止
00526         case 20:
00527             if(stop_flag[8] == 0) {
00528                 stop();
00529                 counter.stop();
00530                 counter.reset();
00531                 counter.start();
00532                 stop_flag[8] = 1;
00533             }
00534             if(counter.read() > 0.5f) {
00535                 phase = 21;
00536                 wheel_reset();
00537             }
00538             break;
00539 
00540         //壁に当たるまで前進
00541         case 21:
00542             if(front_limit == 3) {
00543                 phase = 22;
00544             }
00545             else if(front_limit != 3){
00546                 true_migimae_data[0]     = 0xA0;
00547                 true_migiusiro_data[0]   = 0xA0;
00548                 true_hidarimae_data[0]   = 0xA0;
00549                 true_hidariusiro_data[0] = 0xA0;
00550                 i2c.write(0x10, true_migimae_data,     1, false);
00551                 i2c.write(0x12, true_migiusiro_data,   1, false);
00552                 i2c.write(0x14, true_hidarimae_data,   1, false);
00553                 i2c.write(0x16, true_hidariusiro_data, 1, false);
00554                 wait_us(20);
00555             }
00556             break;
00557 
00558         //シーツ装填
00559         case 22:
00560             YELLOW_LED = 1;
00561             if(start_switch == 1) {
00562                 wheel_reset();
00563                 phase = 23;
00564             } else {
00565                 if(stop_flag[9] == 0) {
00566                     stop();
00567                     counter.stop();
00568                     counter.reset();
00569                     counter.start();
00570                     stop_flag[9] = 1;
00571                 }
00572             }
00573             break;
00574 
00575         //竿のラインまで後進
00576         case 23:
00577             back(-20500);
00578             if((y_pulse1*-1 > 20500) || (y_pulse2*-1 > 20500)) {
00579                 phase = 24;
00580             }
00581             break;
00582 
00583         //1秒停止
00584         case 24:
00585             if(stop_flag[10] == 0) {
00586                 stop();
00587                 counter.stop();
00588                 counter.reset();
00589                 counter.start();
00590                 stop_flag[10] = 1;
00591             }
00592             if(counter.read() > 1.0f) {
00593                 phase = 25;
00594                 wheel_reset();
00595             }
00596             break;
00597 
00598         //ちょっと左移動
00599         case 25:
00600             left(400);
00601             if((x_pulse1 > 400) || (x_pulse2 > 400)) {
00602                 phase = 26;
00603             }
00604             break;
00605 
00606         //1秒停止
00607         case 26:
00608             if(stop_flag[11] == 0) {
00609                 stop();
00610                 counter.stop();
00611                 counter.reset();
00612                 counter.start();
00613                 stop_flag[11] = 1;
00614             }
00615             if(counter.read() > 1.0f) {
00616                 phase = 27;
00617                 wheel_reset();
00618             }
00619             break;
00620 
00621         //90°左旋回
00622         case 27:
00623             turn_left(500);
00624             if(sum_pulse > 500) {
00625                 phase = 28;
00626             }
00627             break;
00628 
00629         //1秒停止
00630         case 28:
00631             if(stop_flag[12] == 0) {
00632                 stop();
00633                 counter.stop();
00634                 counter.reset();
00635                 counter.start();
00636                 stop_flag[12] = 1;
00637             }
00638             if(counter.read() > 1.0f) {
00639                 phase = 29;
00640                 wheel_reset();
00641             }
00642             break;
00643 
00644         //壁に当たるまで後進
00645         case 29:
00646             if(back_limit == 3) {
00647                 phase = 30;
00648             }
00649             else if(back_limit != 3){
00650                 true_migimae_data[0]     = 0x60;
00651                 true_migiusiro_data[0]   = 0x60;
00652                 true_hidarimae_data[0]   = 0x60;
00653                 true_hidariusiro_data[0] = 0x60;
00654                 i2c.write(0x10, true_migimae_data,     1, false);
00655                 i2c.write(0x12, true_migiusiro_data,   1, false);
00656                 i2c.write(0x14, true_hidarimae_data,   1, false);
00657                 i2c.write(0x16, true_hidariusiro_data, 1, false);
00658                 wait_us(20);
00659             }
00660             break;
00661 
00662         //1秒停止
00663         case 30:
00664             if(stop_flag[13] == 0) {
00665                 stop();
00666                 counter.stop();
00667                 counter.reset();
00668                 counter.start();
00669                 stop_flag[13] = 1;
00670             }
00671             if(counter.read() > 1.0f) {
00672                 phase = 31;
00673                 wheel_reset();
00674             }
00675             break;
00676 
00677         //掛けるところまで前進
00678         case 31:
00679             front(9200);
00680             if((y_pulse1 > 9200) || (y_pulse2 > 9200)) {
00681                 phase = 32;
00682                 counter.start();
00683             }
00684             break;
00685 
00686         //1秒停止
00687         case 32:
00688             if(stop_flag[14] == 0) {
00689                 stop();
00690                 counter.stop();
00691                 counter.reset();
00692                 counter.start();
00693                 stop_flag[14] = 1;
00694             }
00695             if(counter.read() > 1.0f) {
00696                 phase = 33;
00697                 wheel_reset();
00698             }
00699             break;
00700 
00701         //妨害防止の左旋回
00702         case 33:
00703             turn_left(20);
00704             if(sum_pulse > 20) {
00705                 phase = 34;
00706             }
00707             break;
00708 
00709         //1秒停止
00710         case 34:
00711             if(stop_flag[15] == 0) {
00712                 stop();
00713                 counter.stop();
00714                 counter.reset();
00715                 counter.start();
00716                 stop_flag[15] = 1;
00717             }
00718             if(counter.read() > 1.0f) {
00719                 phase = 35;
00720                 wheel_reset();
00721             }
00722             break;
00723 
00724         //アームアップ
00725         case 35:
00726             arm_up(36);
00727             if(stop_flag[16] == 0) {
00728                 stop();
00729                 counter.stop();
00730                 counter.reset();
00731                 counter.start();
00732                 stop_flag[16] = 1;
00733             }
00734             if(counter.read() > 2.0f) {
00735                 fan_data[0] = 0xFF;
00736             } else {
00737                 fan_data[0] = 0x80;
00738             }
00739             i2c.write(0x26, fan_data, 1);
00740             i2c.write(0x28, fan_data, 1);
00741             wait_us(20);
00742             break;
00743 
00744         //シーツを掛ける
00745         case 36:
00746             if(stop_flag[17] == 0) {
00747                 counter.stop();
00748                 counter.reset();
00749                 counter.start();
00750                 stop_flag[17] = 1;
00751             }
00752             fan_on(wind_time1, wind_time2, FINAL_PHASE);
00753             break;
00754 
00755         //終了っ!(守衛さん風)
00756         case FINAL_PHASE:
00757         default:
00758             //駆動系統OFF
00759             all_stop();
00760             break;
00761     }
00762 }
00763 
00764 void blue_move(void) {
00765     switch(phase) {
00766 
00767         //スタート位置へセット
00768         case 0:
00769             //スタートスイッチが押されたか
00770             if(start_switch == 1) {
00771                 wheel_reset();
00772                 phase = 1;
00773             }
00774             
00775             //リミットが洗濯物台に触れているか
00776             if(right_limit == 3) {
00777                 USR_LED1 = 1;
00778             } else {
00779                 USR_LED1 = 0;
00780             }
00781             break;
00782 
00783         //回収アームを伸ばす
00784         case 1:
00785             kaisyu_nobasu(2);
00786             //サーボを開いておく
00787             servo_data[0] = 0x03;
00788             i2c.write(0x30, servo_data, 1);
00789             break;
00790 
00791         //1.0秒停止
00792         case 2:
00793             if(stop_flag[0] == 0) {
00794                 stop();
00795                 servo_data[0] = 0x04;
00796                 i2c.write(0x30, servo_data, 1);
00797                 counter.stop();
00798                 counter.reset();
00799                 counter.start();
00800                 stop_flag[0] = 1;
00801             }
00802             if(counter.read() > 1.0f) {
00803                 phase = 3;
00804                 wheel_reset();
00805             }
00806             break;
00807 
00808         //壁に当たるまで前進
00809         case 3:
00810             /*
00811             if(front_limit == 3) {
00812                 phase = 4;
00813             }
00814             else if(front_limit != 3){
00815                 true_migimae_data[0]     = 0xA0;
00816                 true_migiusiro_data[0]   = 0xA0;
00817                 true_hidarimae_data[0]   = 0xA0;
00818                 true_hidariusiro_data[0] = 0xA0;
00819                 i2c.write(0x10, true_migimae_data,     1, false);
00820                 i2c.write(0x12, true_migiusiro_data,   1, false);
00821                 i2c.write(0x14, true_hidarimae_data,   1, false);
00822                 i2c.write(0x16, true_hidariusiro_data, 1, false);
00823                 wait_us(20);
00824             }
00825             */
00826             WDT.start();
00827             front(800);
00828             if((y_pulse1 > 800) || (y_pulse2 > 800)) {
00829                 phase = 4;
00830             }
00831             //WDTで5秒経過したら強制的にphase4に移行
00832             if(WDT.read() > 5.0f) {
00833                 phase = 4;
00834                 wheel_reset();
00835             }
00836             break;
00837 
00838         //0.5秒停止
00839         case 4:
00840             if(stop_flag[1] == 0) {
00841                 stop();
00842                 counter.stop();
00843                 WDT.stop();
00844                 counter.reset();
00845                 WDT.reset();
00846                 counter.start();
00847                 WDT.start();
00848                 stop_flag[1] = 1;
00849             }
00850             if(counter.read() > 0.5f) {
00851                 phase = 5;
00852                 wheel_reset();
00853             }
00854             //WDTで2秒経過したら強制的にphase5に移行
00855             if(WDT.read() > 2.0f) {
00856                 phase = 5;
00857                 wheel_reset();
00858             }
00859             break;
00860 
00861         //回収アーム引っ込める
00862         case 5:
00863             USR_LED3 = 1;
00864             kaisyu_hiku(6);
00865             break;
00866 
00867         //0.5秒停止
00868         case 6:
00869             if(stop_flag[2] == 0) {
00870                 USR_LED4 = 1;
00871                 stop();
00872                 counter.stop();
00873                 counter.reset();
00874                 counter.start();
00875                 stop_flag[2] = 1;
00876             }
00877             if(counter.read() > 0.5f) {
00878                 phase = 7;
00879                 wheel_reset();
00880             }
00881             break;
00882 
00883         //ちょっと後進
00884         case 7:
00885             back(-700);
00886             if((y_pulse1*-1 > 700) || (y_pulse2*-1 > 700)) {
00887                 phase = 8;
00888             }
00889             break;
00890 
00891         //0.5秒停止
00892         case 8:
00893             if(stop_flag[3] == 0) {
00894                 stop();
00895                 counter.stop();
00896                 counter.reset();
00897                 counter.start();
00898                 stop_flag[3] = 1;
00899             }
00900             if(counter.read() > 0.5f) {
00901                 phase = 9;
00902                 wheel_reset();
00903             }
00904             break;
00905 
00906         //左移動
00907         case 9:
00908             left(11500);
00909             if((x_pulse1 > 11500) || (x_pulse2 > 11500)) {
00910                 phase = 10;
00911             }
00912             break;
00913 
00914         //1秒停止
00915         case 10:
00916             if(stop_flag[4] == 0) {
00917                 stop();
00918                 counter.stop();
00919                 counter.reset();
00920                 counter.start();
00921                 stop_flag[4] = 1;
00922             }
00923             if(counter.read() > 1.0f) {
00924                 phase = 11;
00925                 wheel_reset();
00926             }
00927             break;
00928 
00929         //右旋回(180°)
00930         case 11:
00931             counter.reset();
00932             turn_right(950);
00933             if(sum_pulse > 950) {
00934                 phase = 12;
00935             }
00936             break;
00937 
00938         //0.5秒停止
00939         case 12:
00940             if(stop_flag[5] == 0) {
00941                 stop();
00942                 counter.stop();
00943                 counter.reset();
00944                 counter.start();
00945                 stop_flag[5] = 1;
00946             }
00947             if(counter.read() > 0.5f) {
00948                 phase = 13;
00949                 wheel_reset();
00950             }
00951             break;
00952 
00953         //壁に当たるまで後進
00954         case 13:
00955             if(back_limit == 3) {
00956                 phase = 14;
00957             }
00958             else if(back_limit != 3){
00959                 true_migimae_data[0]     = 0x60;
00960                 true_migiusiro_data[0]   = 0x60;
00961                 true_hidarimae_data[0]   = 0x60;
00962                 true_hidariusiro_data[0] = 0x60;
00963                 i2c.write(0x10, true_migimae_data,     1, false);
00964                 i2c.write(0x12, true_migiusiro_data,   1, false);
00965                 i2c.write(0x14, true_hidarimae_data,   1, false);
00966                 i2c.write(0x16, true_hidariusiro_data, 1, false);
00967                 wait_us(20);
00968             }
00969             break;
00970 
00971         //0.5秒停止
00972         case 14:
00973             if(stop_flag[6] == 0) {
00974                 stop();
00975                 counter.stop();
00976                 counter.reset();
00977                 counter.start();
00978                 stop_flag[6] = 1;
00979             }
00980             if(counter.read() > 0.5f) {
00981                 phase = 15;
00982                 wheel_reset();
00983             }
00984             break;
00985 
00986         //壁に当たるまで右移動
00987         case 15:
00988             if(right_limit == 3) {
00989                 phase = 16;
00990             }
00991             else if(right_limit != 3) {
00992                 true_migimae_data[0]     = 0x40;
00993                 true_migiusiro_data[0]   = 0xBF;
00994                 true_hidarimae_data[0]   = 0xBF;
00995                 true_hidariusiro_data[0] = 0x40;
00996                 i2c.write(0x10, true_migimae_data,     1, false);
00997                 i2c.write(0x12, true_migiusiro_data,   1, false);
00998                 i2c.write(0x14, true_hidarimae_data,   1, false);
00999                 i2c.write(0x16, true_hidariusiro_data, 1, false);
01000                 wait_us(20);
01001             }
01002             break;
01003 
01004         //0.5秒停止
01005         case 16:
01006             if(stop_flag[7] == 0) {
01007                 stop();
01008                 counter.stop();
01009                 counter.reset();
01010                 counter.start();
01011                 stop_flag[7] = 1;
01012             }
01013             if(counter.read() > 0.5f) {
01014                 phase = 17;
01015                 wheel_reset();
01016             }
01017             break;
01018 
01019         //排出
01020         case 17:
01021             tyokudo_nobasu(18);
01022             break;
01023 
01024         //前進
01025         case 18:
01026             front(5000);
01027             if((y_pulse1 > 5000) || (y_pulse2 > 5000)) {
01028                 phase = 19;
01029             }
01030             break;
01031 
01032         //0.5秒停止
01033         case 19:
01034             if(stop_flag[8] == 0) {
01035                 stop();
01036                 counter.stop();
01037                 counter.reset();
01038                 counter.start();
01039                 stop_flag[8] = 1;
01040             }
01041             if(counter.read() > 0.5f) {
01042                 phase = 20;
01043                 wheel_reset();
01044             }
01045             break;
01046 
01047         //排出しまう
01048         case 20:
01049             tyokudo_hiku(21);
01050             break;
01051 
01052         //0.5秒停止
01053         case 21:
01054             if(stop_flag[9] == 0) {
01055                 stop();
01056                 counter.stop();
01057                 counter.reset();
01058                 counter.start();
01059                 stop_flag[9] = 1;
01060             }
01061             if(counter.read() > 0.5f) {
01062                 phase = 22;
01063                 wheel_reset();
01064             }
01065             break;
01066 
01067         //壁に当たるまで右移動
01068         case 22:
01069             if(right_limit == 3) {
01070                 phase = 23;
01071             }
01072             else if(right_limit != 3) {
01073                 true_migimae_data[0]     = 0x40;
01074                 true_migiusiro_data[0]   = 0xBF;
01075                 true_hidarimae_data[0]   = 0xBF;
01076                 true_hidariusiro_data[0] = 0x40;
01077                 i2c.write(0x10, true_migimae_data,     1, false);
01078                 i2c.write(0x12, true_migiusiro_data,   1, false);
01079                 i2c.write(0x14, true_hidarimae_data,   1, false);
01080                 i2c.write(0x16, true_hidariusiro_data, 1, false);
01081                 wait_us(20);
01082             }
01083             break;
01084 
01085         //0.5秒停止
01086         case 23:
01087             if(stop_flag[10] == 0) {
01088                 stop();
01089                 counter.stop();
01090                 counter.reset();
01091                 counter.start();
01092                 stop_flag[10] = 1;
01093             }
01094             if(counter.read() > 0.5f) {
01095                 phase = 24;
01096                 wheel_reset();
01097             }
01098             break;
01099 
01100         //壁に当たるまで後進
01101         case 24:
01102             if(back_limit == 3) {
01103                 phase = 25;
01104             }
01105             else if(back_limit != 3){
01106                 true_migimae_data[0]     = 0x60;
01107                 true_migiusiro_data[0]   = 0x60;
01108                 true_hidarimae_data[0]   = 0x60;
01109                 true_hidariusiro_data[0] = 0x60;
01110                 i2c.write(0x10, true_migimae_data,     1, false);
01111                 i2c.write(0x12, true_migiusiro_data,   1, false);
01112                 i2c.write(0x14, true_hidarimae_data,   1, false);
01113                 i2c.write(0x16, true_hidariusiro_data, 1, false);
01114                 wait_us(20);
01115             }
01116             break;
01117 
01118         //シーツ装填
01119         case 25:
01120             YELLOW_LED = 1;
01121             if(start_switch == 1) {
01122                 wheel_reset();
01123                 phase = 26;
01124             } else {
01125                 if(stop_flag[11] == 0) {
01126                     stop();
01127                     counter.stop();
01128                     counter.reset();
01129                     counter.start();
01130                     stop_flag[11] = 1;
01131                 }
01132             }
01133             break;
01134 
01135         //竿のラインまで前進
01136         case 26:
01137             front(21200);
01138             if((y_pulse1 > 21200) || (y_pulse2 > 21200)) {
01139                 phase = 27;
01140             }
01141             break;
01142 
01143         //1秒停止
01144         case 27:
01145             if(stop_flag[12] == 0) {
01146                 stop();
01147                 counter.stop();
01148                 counter.reset();
01149                 counter.start();
01150                 stop_flag[12] = 1;
01151             }
01152             if(counter.read() > 1.0f) {
01153                 phase = 28;
01154                 wheel_reset();
01155             }
01156             break;
01157 
01158         //ちょっと左移動
01159         case 28:
01160             left(400);
01161             if((x_pulse1 > 400) || (x_pulse2 > 400)) {
01162                 phase = 29;
01163             }
01164             break;
01165 
01166         //1秒停止
01167         case 29:
01168             if(stop_flag[13] == 0) {
01169                 stop();
01170                 counter.stop();
01171                 counter.reset();
01172                 counter.start();
01173                 stop_flag[13] = 1;
01174             }
01175             if(counter.read() > 1.0f) {
01176                 phase = 30;
01177                 wheel_reset();
01178             }
01179             break;
01180 
01181         //90°右旋回
01182         case 30:
01183             turn_right(480);
01184             if(sum_pulse > 480) {
01185                 phase = 31;
01186             }
01187             break;
01188 
01189         //1秒停止
01190         case 31:
01191             if(stop_flag[14] == 0) {
01192                 stop();
01193                 counter.stop();
01194                 counter.reset();
01195                 counter.start();
01196                 stop_flag[14] = 1;
01197             }
01198             if(counter.read() > 1.0f) {
01199                 phase = 32;
01200                 wheel_reset();
01201             }
01202             break;
01203 
01204         //壁に当たるまで前進
01205         case 32:
01206             if(front_limit == 3) {
01207                 phase = 33;
01208             }
01209             else if(front_limit != 3){
01210                 true_migimae_data[0]     = 0xA0;
01211                 true_migiusiro_data[0]   = 0xA0;
01212                 true_hidarimae_data[0]   = 0xA0;
01213                 true_hidariusiro_data[0] = 0xA0;
01214                 i2c.write(0x10, true_migimae_data,     1, false);
01215                 i2c.write(0x12, true_migiusiro_data,   1, false);
01216                 i2c.write(0x14, true_hidarimae_data,   1, false);
01217                 i2c.write(0x16, true_hidariusiro_data, 1, false);
01218                 wait_us(20);
01219             }
01220             break;
01221 
01222         //1秒停止
01223         case 33:
01224             if(stop_flag[15] == 0) {
01225                 stop();
01226                 counter.stop();
01227                 counter.reset();
01228                 counter.start();
01229                 stop_flag[15] = 1;
01230             }
01231             if(counter.read() > 1.0f) {
01232                 phase = 34;
01233                 wheel_reset();
01234             }
01235             break;
01236 
01237         //掛けるところまで後進
01238         case 34:
01239             back(-9200);
01240             if((y_pulse1*-1 > 9200) || (y_pulse2*-1 > 9200)) {
01241                 phase = 35;
01242                 counter.start();
01243             }
01244             break;
01245 
01246         //1秒停止
01247         case 35:
01248             if(stop_flag[16] == 0) {
01249                 stop();
01250                 counter.stop();
01251                 counter.reset();
01252                 counter.start();
01253                 stop_flag[16] = 1;
01254             }
01255             if(counter.read() > 1.0f) {
01256                 phase = 36;
01257                 wheel_reset();
01258             }
01259             break;
01260 
01261         //妨害防止の右旋回
01262         case 36:
01263             turn_right(20);
01264             if(sum_pulse > 20) {
01265                 phase = 37;
01266             }
01267             break;
01268 
01269         //1秒停止
01270         case 37:
01271             if(stop_flag[17] == 0) {
01272                 stop();
01273                 counter.stop();
01274                 counter.reset();
01275                 counter.start();
01276                 stop_flag[17] = 1;
01277             }
01278             if(counter.read() > 1.0f) {
01279                 phase = 38;
01280                 wheel_reset();
01281             }
01282             break;
01283 
01284         //アームアップ
01285         case 38:
01286             arm_up(39);
01287             if(stop_flag[18] == 0) {
01288                 stop();
01289                 counter.stop();
01290                 counter.reset();
01291                 counter.start();
01292                 stop_flag[18] = 1;
01293             }
01294             if(counter.read() > 2.0f) {
01295                 fan_data[0] = 0xFF;
01296             } else {
01297                 fan_data[0] = 0x80;
01298             }
01299             i2c.write(0x26, fan_data, 1);
01300             i2c.write(0x28, fan_data, 1);
01301             wait_us(20);
01302             break;
01303 
01304         //シーツを掛ける
01305         case 39:
01306             if(stop_flag[19] == 0) {
01307                 counter.stop();
01308                 counter.reset();
01309                 counter.start();
01310                 stop_flag[19] = 1;
01311             }
01312             fan_on(wind_time1, wind_time2, FINAL_PHASE);
01313             break;
01314 
01315         //終了っ!(守衛さん風)
01316         case FINAL_PHASE:
01317         default:
01318             //駆動系統OFF
01319             all_stop();
01320             break;
01321     }
01322 }
01323 
01324 void init(void) {
01325 
01326     //通信ボーレートの設定
01327     pc.baud(460800);
01328 
01329     limit_serial.baud(115200);
01330 
01331     start_switch.mode(PullUp);
01332     zone_switch.mode(PullDown);
01333     
01334     YELLOW_LED = 0;
01335     USR_LED3 = 0;   USR_LED4 = 0;
01336     
01337     //非常停止関連
01338     pic.baud(19200);
01339     pic.format(8, Serial::None, 1);
01340     pic.attach(get, Serial::RxIrq);
01341 
01342     x_pulse1 = 0;   x_pulse2 = 0;   y_pulse1 = 0;   y_pulse2 = 0;   sum_pulse = 0;
01343     migimae_data[0] = 0x80; migiusiro_data[0] = 0x80;   hidarimae_data[0] = 0x80;   hidariusiro_data[0] = 0x80;
01344     true_migimae_data[0] = 0x80;    true_migiusiro_data[0] = 0x80;  true_hidarimae_data[0] = 0x80;  true_hidariusiro_data[0] = 0x80;
01345     fan_data[0] = 0x80;
01346     servo_data[0] = 0x80;
01347     arm_motor[0] = 0x80;    drop_motor[0] = 0x80;
01348     right_arm_data[0] = 0x80;   left_arm_data[0] = 0x80;
01349 }
01350 
01351 void init_send(void) {
01352 
01353     init_send_data[0] = 0x80;
01354     i2c.write(0x10, init_send_data, 1);
01355     i2c.write(0x12, init_send_data, 1);
01356     i2c.write(0x14, init_send_data, 1);
01357     i2c.write(0x16, init_send_data, 1);
01358     i2c.write(0x18, init_send_data, 1);
01359     i2c.write(0x20, init_send_data, 1);
01360     i2c.write(0x22, init_send_data, 1);
01361     i2c.write(0x24, init_send_data, 1);
01362     i2c.write(0x30, init_send_data, 1);
01363     wait(0.1);
01364 }
01365 
01366 void get(void) {
01367 
01368     baff = pic.getc();
01369 
01370     for(; flug; flug--)
01371         RDATA = baff;
01372 
01373     if(baff == ':')
01374         flug = 1;
01375 }
01376 
01377 void get_pulses(void) {
01378 
01379     x_pulse1 = wheel_x1.getPulses();
01380     x_pulse2 = wheel_x2.getPulses();
01381     y_pulse1 = wheel_y1.getPulses();
01382     y_pulse2 = wheel_y2.getPulses();
01383     sum_pulse = (abs(x_pulse1) + abs(x_pulse2) + abs(y_pulse1) + abs(y_pulse2)) / 4;
01384     arm_pulse = arm_enc.getPulses();
01385 }
01386 
01387 void print_pulses(void) {
01388     
01389     //pc.printf("p: %d, kp: %d, pulse: %d\n\r", phase, kaisyu_phase, arm_pulse);
01390     //pc.printf("p: %d, k_p: %d, pulse: %d\n\r", phase, kaisyu_phase, arm_pulse);
01391     //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);
01392     //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, 
01393     //tyokudo_mae_limit, tyokudo_usiro_limit, right_arm_upper_limit, left_arm_upper_limit);
01394     //pc.printf("%r: %x, l: %x\n\r", right_arm_data[0], left_arm_data[0]);
01395     //pc.printf("limit: 0x%x, upper: 0x%x, lower: 0x%x\n\r", limit_data, upper_limit_data, lower_limit_data);
01396     //pc.printf("x1: %d, x2: %d, y1: %d, y2: %d, phase: %d\n\r", x_pulse1, x_pulse2, y_pulse1, y_pulse2, phase);
01397     //pc.printf("RF: %x, RB: %x, LF: %x, LB: %x, phase: %d\n\r", true_migimae_data[0], true_migiusiro_data[0], true_hidarimae_data[0], true_hidariusiro_data[0], phase);
01398 }
01399 
01400 void get_emergency(void) {
01401 
01402     if(RDATA == '1') {
01403         myled = 1;
01404         emergency = 1;
01405     }
01406     else if(RDATA == '9'){
01407         myled = 0.2;
01408         emergency = 0;
01409         /*
01410         //終了phaseで駆動系統OFF
01411         if(phase == FINAL_PHASE) {
01412             emergency = 1;
01413         } else {
01414             emergency = 0;
01415         }
01416         */
01417     }
01418 }
01419 
01420 void read_limit(void) {
01421 
01422     limit_data = limit_serial.getc();
01423 
01424     //上位1bitが1ならば下のリミットのデータだと判断
01425     if((limit_data & 0b10000000) == 0b10000000) {
01426         lower_limit_data = limit_data;
01427 
01428     //上位1bitが0ならば上のリミットのデータだと判断
01429     } else {
01430         upper_limit_data = limit_data;
01431     }
01432 
01433     //下リミット基板からのデータのマスク処理
01434     masked_lower_front_limit_data = lower_limit_data & 0b00000011;
01435     masked_lower_back_limit_data  = lower_limit_data & 0b00001100;
01436     masked_lower_right_limit_data = lower_limit_data & 0b00110000;
01437     masked_kaisyu_mae_limit_data      = lower_limit_data & 0b01000000;
01438 
01439     //上リミット基板からのデータのマスク処理
01440     //masked_right_arm_lower_limit_data = upper_limit_data & 0b00000001;
01441     masked_kaisyu_usiro_limit_data    = upper_limit_data & 0b00000001;
01442     masked_right_arm_upper_limit_data = upper_limit_data & 0b00000010;
01443     masked_left_arm_lower_limit_data  = upper_limit_data & 0b00000100;
01444     masked_left_arm_upper_limit_data  = upper_limit_data & 0b00001000;
01445     masked_tyokudo_mae_limit_data     = upper_limit_data & 0b00010000;
01446     masked_tyokudo_usiro_limit_data   = upper_limit_data & 0b00100000;
01447         
01448     //前部リミット
01449     switch(masked_lower_front_limit_data) {
01450         //両方押された
01451         case 0x00:
01452             front_limit = 3;
01453             break;
01454         //右が押された
01455         case 0b00000010:
01456             front_limit = 1;
01457             break;
01458         //左が押された
01459         case 0b00000001:
01460             front_limit = 2;
01461             break;
01462         default:
01463             front_limit = 0;
01464             break;
01465     }
01466 
01467     //後部リミット
01468     switch(masked_lower_back_limit_data) {
01469         //両方押された
01470         case 0x00:
01471             back_limit = 3;
01472             break;
01473         //右が押された
01474         case 0b00001000:
01475             back_limit = 1;
01476             break;
01477         //左が押された
01478         case 0b00000100:
01479             back_limit = 2;
01480             break;
01481         default:
01482             back_limit = 0;
01483             break;
01484     }
01485 
01486     //右部リミット
01487     switch(masked_lower_right_limit_data) {
01488         //両方押された
01489         case 0x00:
01490             right_limit = 3;
01491             break;
01492         //右が押された
01493         case 0b00100000:
01494             right_limit = 1;
01495             break;
01496         //左が押された
01497         case 0b00010000:
01498             right_limit = 2;
01499             break;
01500         default:
01501             right_limit = 0;
01502             break;
01503     }
01504 
01505     //回収機構リミット
01506     switch(masked_kaisyu_mae_limit_data) {
01507         //押された
01508         case 0b00000000:
01509             kaisyu_mae_limit = 1;
01510             break;
01511         case 0b01000000:
01512             kaisyu_mae_limit = 0;
01513             break;
01514         default:
01515             kaisyu_mae_limit = 0;
01516             break;
01517     }
01518 
01519     //右腕下部リミット
01520     /*
01521     switch(masked_right_arm_lower_limit_data) {
01522         //押された
01523         case 0b00000000:
01524             right_arm_lower_limit = 1;
01525             break;
01526         case 0b00000001:
01527             right_arm_lower_limit = 0;
01528             break;
01529         default:
01530             right_arm_lower_limit = 0;
01531             break;
01532     }
01533     */
01534         
01535     //回収後リミット
01536     switch(masked_kaisyu_usiro_limit_data) {
01537         case 0b00000000:
01538             kaisyu_usiro_limit = 1;
01539             break;
01540         case 0b00000001:
01541             kaisyu_usiro_limit = 0;
01542             break;
01543         default:
01544             kaisyu_usiro_limit = 0;
01545             break;
01546     }
01547         
01548     //右腕上部リミット
01549     switch(masked_right_arm_upper_limit_data) {
01550         //押された
01551         case 0b00000000:
01552             right_arm_upper_limit = 1;
01553             break;
01554         case 0b00000010:
01555             right_arm_upper_limit = 0;
01556             break;
01557         default:
01558             right_arm_upper_limit = 0;
01559             break;
01560     }
01561 
01562     //左腕下部リミット
01563     switch(masked_left_arm_lower_limit_data) {
01564         //押された
01565         case 0b00000000:
01566             left_arm_lower_limit = 1;
01567             break;
01568         case 0b00000100:
01569             left_arm_lower_limit = 0;
01570             break;
01571         default:
01572             left_arm_lower_limit = 0;
01573             break;
01574     }
01575 
01576     //左腕上部リミット
01577     switch(masked_left_arm_upper_limit_data) {
01578         //押された
01579         case 0b00000000:
01580             left_arm_upper_limit = 1;
01581             break;
01582         case 0b00001000:
01583             left_arm_upper_limit = 0;
01584             break;
01585         default:
01586             left_arm_upper_limit = 0;
01587             break;
01588     }
01589 
01590     //直動の前
01591     switch(masked_tyokudo_mae_limit_data) {
01592         //押された
01593         case 0b00000000:
01594             tyokudo_mae_limit = 1;
01595             break;
01596         case 0b00010000:
01597             tyokudo_mae_limit = 0;
01598             break;
01599         default:
01600             tyokudo_mae_limit = 0;
01601             break;
01602     }
01603 
01604     //直動の後
01605     switch(masked_tyokudo_usiro_limit_data) {
01606         //押された
01607         case 0b00000000:
01608             tyokudo_usiro_limit = 1;
01609             break;
01610         case 0b00100000:
01611             tyokudo_usiro_limit = 0;
01612             break;
01613         default:
01614             tyokudo_usiro_limit = 0;
01615             break;
01616     }
01617 }
01618 
01619 void wheel_reset(void) {
01620 
01621     wheel_x1.reset();
01622     wheel_x2.reset();
01623     wheel_y1.reset();
01624     wheel_y2.reset();
01625 }
01626 
01627 void kaisyu(int pulse, int next_phase) {
01628 
01629     switch (kaisyu_phase) {
01630 
01631         case 0:
01632             //前進->減速
01633             //3000pulseまで高速前進
01634             if(pulse < 3000) {
01635                 arm_motor[0] = 0xFF;
01636                 //kaisyu_phase = 1;
01637             }
01638 
01639             //3000pulse超えたら低速前進
01640             else if(pulse >= 3000) {
01641                 arm_motor[0] = 0xB3;
01642                 kaisyu_phase = 1;
01643             }
01644             break;
01645 
01646         case 1:
01647             //前進->停止->後進
01648             //3600pulseまで低速前進
01649             if(pulse < 3600) {
01650                 arm_motor[0] = 0xB3;
01651                 //kaisyu_phase = 2;
01652             }
01653 
01654             //3600pulse超えたら停止
01655             else if(pulse >= 3600) {
01656                 arm_motor[0] = 0x80;
01657 
01658                 //1秒待ってから引っ込める
01659                 counter.start();
01660                 if(counter.read() > 1.0f) {
01661                     kaisyu_phase = 2;
01662                 }
01663             }
01664             //後ろのリミットが押されたら強制停止
01665             if(kaisyu_usiro_limit == 1) {
01666                 arm_motor[0] = 0x80;
01667                 //1秒待ってから引っ込める
01668                 counter.start();
01669                 if(counter.read() > 1.0f) {
01670                     kaisyu_phase = 2;
01671                 }            
01672             }
01673             break;
01674 
01675         case 2:
01676             //後進->減速
01677             //500pulseまで高速後進
01678             counter.reset();
01679             if(pulse > 500) {
01680                 arm_motor[0] = 0x00;
01681                 //kaisyu_phase = 3;
01682 
01683             }
01684             //500pulse以下になったら低速後進
01685             else if(pulse <= 500) {
01686                 arm_motor[0] = 0x4C;
01687                 kaisyu_phase = 3;
01688             }
01689             break;
01690 
01691         case 3:
01692             //後進->停止
01693             //リミット押されるまで低速後進
01694             if(pulse <= 500) {
01695                 arm_motor[0] = 0x4C;
01696                 //kaisyu_phase = 4;
01697             }
01698 
01699             //リミット押されたら停止
01700             if(kaisyu_mae_limit == 1) {
01701                 arm_motor[0] = 0x80;
01702                 kaisyu_phase = 4;
01703                 phase = next_phase;
01704             }
01705             break;
01706 
01707         default:
01708             arm_motor[0] = 0x80;
01709             break;
01710     }
01711     //回収MDへ書き込み
01712     i2c.write(0x18, arm_motor, 1);
01713     wait_us(20);
01714 }
01715 
01716 void kaisyu_nobasu(int next_phase) {
01717     
01718     //前進->減速
01719     //3000pulseまで高速前進
01720     if(arm_pulse < 3000) {
01721         arm_motor[0] = 0xFF;
01722     }
01723     //3000pulse超えたら低速前進
01724     else if(arm_pulse >= 3000) {
01725         arm_motor[0] = 0xB3;
01726     }
01727     //3600pulse超えたら停止
01728     else if(arm_pulse >= 3600) {
01729         arm_motor[0] = 0x80;
01730         phase = next_phase;
01731     } else {
01732         arm_motor[0] = 0x80;
01733     }
01734     
01735     //後ろのリミットが押されたら強制停止
01736     if(kaisyu_usiro_limit == 1) {
01737         arm_motor[0] = 0x80;
01738         phase = next_phase;
01739     }
01740     //回収MDへ書き込み
01741     i2c.write(0x18, arm_motor, 1);
01742     wait_us(20);
01743 }
01744 
01745 void kaisyu_hiku(int next_phase) {
01746     
01747     //後進->減速
01748     //500pulseより大きい範囲で高速後進
01749     if(arm_pulse > 500) {
01750         arm_motor[0] = 0x00;
01751     }
01752     //500pulse以下になったら低速後進
01753     else if(arm_pulse <= 500) {
01754         arm_motor[0] = 0x4C;
01755     }
01756     //0pulse以下で停止
01757     else if(arm_pulse <= 0) {
01758         arm_motor[0] = 0x80;
01759         phase = next_phase;
01760     } else {
01761         arm_motor[0] = 0x80;
01762     }
01763     
01764     //後ろのリミットが押されたら強制停止
01765     if(kaisyu_mae_limit == 1) {
01766         arm_motor[0] = 0x80;
01767         phase = next_phase;
01768     }
01769     //回収MDへ書き込み
01770     i2c.write(0x18, arm_motor, 1);
01771     wait_us(20);
01772 }
01773 
01774 void tyokudo(int pulse, int next_phase) {
01775 
01776     switch(tyokudo_phase) {
01777 
01778         case 0:
01779             //前進->減速
01780 
01781             /*   エンコーダー読まずにリミットだけ(修正必須)   */
01782             //3600pulseより大きい&直堂前リミットが押されたら次のphaseへ移行
01783             if(tyokudo_mae_limit == 0) {
01784                 //2000pulseまで高速前進
01785                 if(pulse < 2000) {
01786                     arm_motor[0] = 0xC0;
01787                     drop_motor[0] = 0xE6;
01788                 }
01789                 //2000pulse以上で低速前進
01790                 else if(pulse >= 2000) {
01791                     arm_motor[0] = 0xC0;
01792                     drop_motor[0] = 0xE6;
01793                 }
01794                 //パルスが3600を終えたらアームのみ強制停止
01795                 else if(pulse > 3600) {
01796                     arm_motor[0] = 0x80;
01797                     drop_motor[0] = 0xE6;
01798                 }
01799                 
01800                 //後ろのリミットが押されたら強制停止
01801                 if(kaisyu_usiro_limit == 1) {
01802                     arm_motor[0] = 0x80;
01803                 }
01804             }
01805 
01806             //直動の前リミットが押されたら
01807             else if(tyokudo_mae_limit == 1) {
01808                 //高速後進
01809                 arm_motor[0] = 0x4C;
01810                 drop_motor[0] = 0x00;
01811                 tyokudo_phase = 1;
01812             }
01813             break;
01814 
01815         case 1:
01816             //後進->停止
01817             if(tyokudo_usiro_limit == 1) {
01818                 drop_motor[0] = 0x80;
01819                 
01820                 if(kaisyu_mae_limit == 1) {
01821                     arm_motor[0] = 0x80;
01822                     tyokudo_phase = 2;
01823                     phase = next_phase;
01824                 }
01825             }
01826             if(kaisyu_mae_limit == 1) {
01827                 arm_motor[0] = 0x80;
01828                 
01829                 if(tyokudo_usiro_limit == 1) {
01830                     drop_motor[0] = 0x80;
01831                     tyokudo_phase = 2;
01832                     phase = next_phase;
01833                 }
01834             }
01835             break;
01836             
01837         default:
01838             arm_motor[0] = 0x80;
01839             drop_motor[0] = 0x80;
01840             break;
01841     }
01842     //回収MD・排出MDへ書き込み
01843     i2c.write(0x18, arm_motor,  1);
01844     i2c.write(0x20, drop_motor, 1);
01845     wait_us(20);
01846 }
01847 
01848 void tyokudo_nobasu(int next_phase) {
01849     
01850     if(tyokudo_mae_limit == 0) {
01851         
01852         drop_motor[0] = 0xFF;
01853 
01854         //2000pulseまで高速前進
01855         if(arm_pulse < 2000) {
01856             arm_motor[0] = 0xFF;
01857         }
01858         //2000pulse以上で低速前進
01859         else if(arm_pulse >= 2000) {
01860             arm_motor[0] = 0xB3;
01861         }
01862         //パルスが2500を終えたらアームのみ強制停止
01863         else if(arm_pulse > 2500) {
01864             arm_motor[0] = 0x80;
01865         } else {
01866             arm_motor[0] = 0x80;
01867         }
01868                 
01869         //後ろのリミットが押されたら強制停止
01870         if(kaisyu_usiro_limit == 1) {
01871             arm_motor[0] = 0x80;
01872         }
01873     }
01874     else if(tyokudo_mae_limit == 1) {
01875         
01876         drop_motor[0] = 0x80;
01877         
01878         //2000pulseまで高速前進
01879         if(arm_pulse < 2000) {
01880             arm_motor[0] = 0xFF;
01881         }
01882         //2000pulse以上で低速前進
01883         else if(arm_pulse >= 2000) {
01884             arm_motor[0] = 0xB3;
01885         }
01886         //パルスが2500を終えたらアームのみ強制停止
01887         else if(arm_pulse > 2500) {
01888             arm_motor[0] = 0x80;
01889             phase = next_phase;
01890         } else {
01891             arm_motor[0] = 0x80;
01892         }
01893         
01894         //後ろのリミットが押されたら強制停止
01895         if(kaisyu_usiro_limit == 1) {
01896             arm_motor[0] = 0x80;
01897             phase = next_phase;
01898         }
01899     }
01900     //回収MD・排出MDへ書き込み
01901     i2c.write(0x18, arm_motor,  1);
01902     i2c.write(0x20, drop_motor, 1);
01903     wait_us(20);
01904 }
01905 
01906 void tyokudo_hiku(int next_phase) {
01907     
01908     if(tyokudo_usiro_limit == 0) {
01909         
01910         drop_motor[0] = 0x00;
01911 
01912         //500pulseより大きい範囲で高速後進
01913         if(arm_pulse > 500) {
01914             arm_motor[0] = 0x00;
01915         }
01916         //500pulse以下になったら低速後進
01917         else if(arm_pulse <= 500) {
01918             arm_motor[0] = 0x4C;
01919         }
01920         //0pulse以下で停止
01921         else if(arm_pulse <= 0) {
01922             arm_motor[0] = 0x80;
01923         } else {
01924             arm_motor[0] = 0x80;
01925         }
01926         
01927         //後ろのリミットが押されたら強制停止
01928         if(kaisyu_mae_limit == 1) {
01929             arm_motor[0] = 0x80;
01930         }
01931     }
01932     else if(tyokudo_usiro_limit == 1) {
01933         
01934         drop_motor[0] = 0x80;
01935         
01936         //500pulseより大きい範囲で高速後進
01937         if(arm_pulse > 500) {
01938             arm_motor[0] = 0x00;
01939         }
01940         //500pulse以下になったら低速後進
01941         else if(arm_pulse <= 500) {
01942             arm_motor[0] = 0x4C;
01943         }
01944         //0pulse以下で停止
01945         else if(arm_pulse <= 0) {
01946             arm_motor[0] = 0x80;
01947             phase = next_phase;
01948         } else {
01949             arm_motor[0] = 0x80;
01950         }
01951         
01952         //後ろのリミットが押されたら強制停止
01953         if(kaisyu_mae_limit == 1) {
01954             arm_motor[0] = 0x80;
01955             phase = next_phase;
01956         }
01957     }
01958     //回収MD・排出MDへ書き込み
01959     i2c.write(0x18, arm_motor,  1);
01960     i2c.write(0x20, drop_motor, 1);
01961     wait_us(20);
01962 }
01963 
01964 void arm_up(int next_phase) {
01965     
01966     //両腕、上限リミットが押されてなかったら上昇
01967     if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 0)) {
01968         right_arm_data[0] = 0x00;   left_arm_data[0] = 0x00;
01969     }
01970     //右腕のみリミットが押されたら左腕のみ上昇
01971     else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 0)) {
01972         right_arm_data[0] = 0x80;   left_arm_data[0] = 0x00;
01973     }
01974     //左腕のみリミットが押されたら右腕のみ上昇
01975     else if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 1)) {
01976         right_arm_data[0] = 0x00;   left_arm_data[0] = 0x80;
01977     }
01978     //両腕、上限リミットが押されたら停止
01979     else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 1)) {
01980         right_arm_data[0] = 0x80;   left_arm_data[0] = 0x80;
01981         phase = next_phase;
01982     }
01983     i2c.write(0x22, right_arm_data, 1);
01984     i2c.write(0x24, left_arm_data, 1);
01985     wait_us(20);
01986 }
01987 
01988 void fan_on(float first_wind_time, float second_wind_time, int next_phase) {
01989     
01990     if(counter.read() <= first_wind_time) {
01991         fan_data[0] = 0xFF;
01992         servo_data[0] = 0x04;
01993     }
01994     else if((counter.read() > first_wind_time) && (counter.read() <= first_wind_time + second_wind_time)) {
01995         fan_data[0] = 0xFF;
01996         servo_data[0] = 0x03;
01997     }
01998     else if(counter.read() > first_wind_time + second_wind_time) {
01999         fan_data[0] = 0x80;
02000         servo_data[0] = 0x04;
02001         phase = next_phase;
02002     }
02003     i2c.write(0x26, fan_data, 1);
02004     i2c.write(0x28, fan_data, 1);
02005     i2c.write(0x30, servo_data, 1);
02006     wait_us(20);
02007 }
02008 
02009 void front(int target) {
02010 
02011     front_PID(target);
02012     i2c.write(0x10, true_migimae_data,     1, false);
02013     i2c.write(0x12, true_migiusiro_data,   1, false);
02014     i2c.write(0x14, true_hidarimae_data,   1, false);
02015     i2c.write(0x16, true_hidariusiro_data, 1, false);
02016     wait_us(20);
02017 }
02018 
02019 void back(int target) {
02020 
02021     back_PID(target);
02022     i2c.write(0x10, true_migimae_data,     1, false);
02023     i2c.write(0x12, true_migiusiro_data,   1, false);
02024     i2c.write(0x14, true_hidarimae_data,   1, false);
02025     i2c.write(0x16, true_hidariusiro_data, 1, false);
02026     wait_us(20);
02027 }
02028 
02029 void right(int target) {
02030 
02031     right_PID(target);
02032     i2c.write(0x10, true_migimae_data,     1, false);
02033     i2c.write(0x12, true_migiusiro_data,   1, false);
02034     i2c.write(0x14, true_hidarimae_data,   1, false);
02035     i2c.write(0x16, true_hidariusiro_data, 1, false);
02036     wait_us(20);
02037 }
02038 
02039 void left(int target) {
02040 
02041     left_PID(target);
02042     i2c.write(0x10, true_migimae_data,     1, false);
02043     i2c.write(0x12, true_migiusiro_data,   1, false);
02044     i2c.write(0x14, true_hidarimae_data,   1, false);
02045     i2c.write(0x16, true_hidariusiro_data, 1, false);
02046     wait_us(20);
02047 }
02048 
02049 void turn_right(int target) {
02050 
02051     turn_right_PID(target);
02052     i2c.write(0x10, true_migimae_data,     1, false);
02053     i2c.write(0x12, true_migiusiro_data,   1, false);
02054     i2c.write(0x14, true_hidarimae_data,   1, false);
02055     i2c.write(0x16, true_hidariusiro_data, 1, false);
02056     wait_us(20);
02057 }
02058 
02059 void turn_left(int target) {
02060 
02061     turn_left_PID(target);
02062     i2c.write(0x10, true_migimae_data,     1, false);
02063     i2c.write(0x12, true_migiusiro_data,   1, false);
02064     i2c.write(0x14, true_hidarimae_data,   1, false);
02065     i2c.write(0x16, true_hidariusiro_data, 1, false);
02066     wait_us(20);
02067 }
02068 
02069 void stop(void) {
02070 
02071     true_migimae_data[0]     = 0x80;
02072     true_migiusiro_data[0]   = 0x80;
02073     true_hidarimae_data[0]   = 0x80;
02074     true_hidariusiro_data[0] = 0x80;
02075 
02076     i2c.write(0x10, true_migimae_data,     1, false);
02077     i2c.write(0x12, true_migiusiro_data,   1, false);
02078     i2c.write(0x14, true_hidarimae_data,   1, false);
02079     i2c.write(0x16, true_hidariusiro_data, 1, false);
02080     wait_us(20);
02081 }
02082 
02083 void all_stop(void) {
02084     
02085     true_migimae_data[0]     = 0x80;
02086     true_migiusiro_data[0]   = 0x80;
02087     true_hidarimae_data[0]   = 0x80;
02088     true_hidariusiro_data[0] = 0x80;
02089     arm_motor[0]  = 0x80;
02090     drop_motor[0] = 0x80;
02091     right_arm_data[0] = 0x80;
02092     left_arm_data[0]  = 0x80;
02093     fan_data[0]   = 0x80;
02094     servo_data[0] = 0x04;
02095                         
02096     i2c.write(0x10, true_migimae_data,     1, false);
02097     i2c.write(0x12, true_migiusiro_data,   1, false);
02098     i2c.write(0x14, true_hidarimae_data,   1, false);
02099     i2c.write(0x16, true_hidariusiro_data, 1, false);
02100     i2c.write(0x18, arm_motor,  1);
02101     i2c.write(0x20, drop_motor, 1);
02102     i2c.write(0x22, right_arm_data, 1);
02103     i2c.write(0x24, left_arm_data, 1);
02104     i2c.write(0x26, fan_data, 1);
02105     i2c.write(0x28, fan_data, 1);
02106     i2c.write(0x30, servo_data, 1);
02107     wait_us(20);
02108 }
02109 
02110 void front_PID(int target) {
02111 
02112     //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
02113     front_migimae.setInputLimits(-2147483648,     2147483647);
02114     front_migiusiro.setInputLimits(-2147483648,   2147483647);
02115     front_hidarimae.setInputLimits(-2147483648,   2147483647);
02116     front_hidariusiro.setInputLimits(-2147483648, 2147483647);
02117 
02118     //制御量の最小、最大
02119     //正転(目標に達してない)
02120     if((y_pulse1 < target) && (y_pulse2 < target)) {
02121         front_migimae.setOutputLimits(0x84,     0xF5);
02122         front_migiusiro.setOutputLimits(0x84,   0xF5);
02123         front_hidarimae.setOutputLimits(0x84,   0xFF);
02124         front_hidariusiro.setOutputLimits(0x84, 0xFF);
02125     }
02126     //停止(目標より行き過ぎ)
02127     else if((y_pulse1 > target) && (y_pulse2 > target)) {
02128         front_migimae.setOutputLimits(0x7C,     0x83);
02129         front_migiusiro.setOutputLimits(0x7C,   0x83);
02130         front_hidarimae.setOutputLimits(0x7C,   0x83);
02131         front_hidariusiro.setOutputLimits(0x7C, 0x83);
02132     }
02133 
02134     //よくわからんやつ
02135     front_migimae.setMode(AUTO_MODE);
02136     front_migiusiro.setMode(AUTO_MODE);
02137     front_hidarimae.setMode(AUTO_MODE);
02138     front_hidariusiro.setMode(AUTO_MODE);
02139 
02140     //目標値
02141     front_migimae.setSetPoint(target);
02142     front_migiusiro.setSetPoint(target);
02143     front_hidarimae.setSetPoint(target);
02144     front_hidariusiro.setSetPoint(target);
02145 
02146     //センサ出力
02147     front_migimae.setProcessValue(y_pulse1);
02148     front_migiusiro.setProcessValue(y_pulse1);
02149     front_hidarimae.setProcessValue(y_pulse2);
02150     front_hidariusiro.setProcessValue(y_pulse2);
02151 
02152     //制御量(計算結果)
02153     migimae_data[0]      = front_migimae.compute();
02154     migiusiro_data[0]    = front_migiusiro.compute();
02155     hidarimae_data[0]    = front_hidarimae.compute();
02156     hidariusiro_data[0]  = front_hidariusiro.compute();
02157 
02158     //制御量をPWM値に変換
02159     //正転(目標に達してない)
02160     if((y_pulse1 < target) && (y_pulse2 < target)) {
02161         true_migimae_data[0]     = migimae_data[0];
02162         true_migiusiro_data[0]   = migiusiro_data[0];
02163         true_hidarimae_data[0]   = hidarimae_data[0];
02164         true_hidariusiro_data[0] = hidariusiro_data[0];
02165     }
02166     //停止(目標より行き過ぎ)
02167     else if((y_pulse1 > target) && (y_pulse2 > target)) {
02168         true_migimae_data[0]     = 0x80;
02169         true_migiusiro_data[0]   = 0x80;
02170         true_hidarimae_data[0]   = 0x80;
02171         true_hidariusiro_data[0] = 0x80;
02172     }
02173 }
02174 
02175 void back_PID(int target) {
02176 
02177         //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
02178         back_migimae.setInputLimits(-2147483648,     2147483647);
02179         back_migiusiro.setInputLimits(-2147483648,   2147483647);
02180         back_hidarimae.setInputLimits(-2147483648,   2147483647);
02181         back_hidariusiro.setInputLimits(-2147483648, 2147483647);
02182 
02183         //制御量の最小、最大
02184         //逆転(目標に達してない)
02185         if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 < target*-1)) {
02186             back_migimae.setOutputLimits(0x00,     0x7B);
02187             back_migiusiro.setOutputLimits(0x00,   0x7B);
02188             back_hidarimae.setOutputLimits(0x00,   0x70);
02189             back_hidariusiro.setOutputLimits(0x00, 0x70);
02190             //back_hidarimae.setOutputLimits(0x00,   0x7B);
02191             //back_hidariusiro.setOutputLimits(0x00, 0x7B);
02192         }
02193         //停止(目標より行き過ぎ)
02194         else if((y_pulse1*-1 > target*-1) && (y_pulse2*-1 > target*-1)) {
02195             back_migimae.setOutputLimits(0x7C,     0x83);
02196             back_migiusiro.setOutputLimits(0x7C,   0x83);
02197             back_hidarimae.setOutputLimits(0x7C,   0x83);
02198             back_hidariusiro.setOutputLimits(0x7C, 0x83);
02199         }
02200 
02201         //よくわからんやつ
02202         back_migimae.setMode(AUTO_MODE);
02203         back_migiusiro.setMode(AUTO_MODE);
02204         back_hidarimae.setMode(AUTO_MODE);
02205         back_hidariusiro.setMode(AUTO_MODE);
02206 
02207         //目標値
02208         back_migimae.setSetPoint(target*-1);
02209         back_migiusiro.setSetPoint(target*-1);
02210         back_hidarimae.setSetPoint(target*-1);
02211         back_hidariusiro.setSetPoint(target*-1);
02212 
02213         //センサ出力
02214         back_migimae.setProcessValue(y_pulse1*-1);
02215         back_migiusiro.setProcessValue(y_pulse1*-1);
02216         back_hidarimae.setProcessValue(y_pulse2*-1);
02217         back_hidariusiro.setProcessValue(y_pulse2*-1);
02218 
02219         //制御量(計算結果)
02220         migimae_data[0]      = back_migimae.compute();
02221         migiusiro_data[0]    = back_migiusiro.compute();
02222         hidarimae_data[0]    = back_hidarimae.compute();
02223         hidariusiro_data[0]  = back_hidariusiro.compute();
02224 
02225         //制御量をPWM値に変換
02226         //逆転(目標に達してない)
02227         if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 < target*-1)) {
02228             true_migimae_data[0]     = 0x7B - migimae_data[0];
02229             true_migiusiro_data[0]   = 0x7B - migiusiro_data[0];
02230             true_hidarimae_data[0]   = 0x7B - hidarimae_data[0];
02231             true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
02232         }
02233         //停止(目標より行き過ぎ)
02234         else if((y_pulse1*-1 > target*-1) && (y_pulse2*-1 > target*-1)) {
02235             true_migimae_data[0]     = 0x80;
02236             true_migiusiro_data[0]   = 0x80;
02237             true_hidarimae_data[0]   = 0x80;
02238             true_hidariusiro_data[0] = 0x80;
02239         }
02240 }
02241 
02242 void right_PID(int target) {
02243 
02244         //センサ出力値の最小、最大
02245         right_migimae.setInputLimits(-2147483648,     2147483647);
02246         right_migiusiro.setInputLimits(-2147483648,   2147483647);
02247         right_hidarimae.setInputLimits(-2147483648,   2147483647);
02248         right_hidariusiro.setInputLimits(-2147483648, 2147483647);
02249 
02250         //制御量の最小、最大
02251         //右進(目標まで達していない)
02252         if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 < target*-1)) {
02253             right_migimae.setOutputLimits(0x6A,     0x6C);
02254             //right_migimae.setOutputLimits(0x7A,     0x7B);
02255             right_migiusiro.setOutputLimits(0xFE,   0xFF);
02256             right_hidarimae.setOutputLimits(0xEF,   0xF0);
02257             //right_hidarimae.setOutputLimits(0xFE,   0xFF);
02258             right_hidariusiro.setOutputLimits(0x7A, 0x7B);
02259 
02260         }
02261         //停止(目標より行き過ぎ)
02262         else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 > target*-1)) {
02263             right_migimae.setOutputLimits(0x7C,     0x83);
02264             right_migiusiro.setOutputLimits(0x7C,   0x83);
02265             right_hidarimae.setOutputLimits(0x7C,   0x83);
02266             right_hidariusiro.setOutputLimits(0x7C, 0x83);
02267         }
02268 
02269         //よくわからんやつ
02270         right_migimae.setMode(AUTO_MODE);
02271         right_migiusiro.setMode(AUTO_MODE);
02272         right_hidarimae.setMode(AUTO_MODE);
02273         right_hidariusiro.setMode(AUTO_MODE);
02274 
02275         //目標値
02276         right_migimae.setSetPoint(target*-1);
02277         right_migiusiro.setSetPoint(target*-1);
02278         right_hidarimae.setSetPoint(target*-1);
02279         right_hidariusiro.setSetPoint(target*-1);
02280 
02281         //センサ出力
02282         right_migimae.setProcessValue(x_pulse1*-1);
02283         right_migiusiro.setProcessValue(x_pulse2*-1);
02284         right_hidarimae.setProcessValue(x_pulse1*-1);
02285         right_hidariusiro.setProcessValue(x_pulse2*-1);
02286 
02287         //制御量(計算結果)
02288         migimae_data[0]      = right_migimae.compute();
02289         migiusiro_data[0]    = right_migiusiro.compute();
02290         hidarimae_data[0]    = right_hidarimae.compute();
02291         hidariusiro_data[0]  = right_hidariusiro.compute();
02292 
02293         //制御量をPWM値に変換
02294         //右進(目標まで達していない)
02295         if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 < target*-1)) {
02296             true_migimae_data[0]     = 0x7B - migimae_data[0];
02297             true_migiusiro_data[0]   = migiusiro_data[0];
02298             true_hidarimae_data[0]   = hidarimae_data[0];
02299             true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
02300         }
02301         //左進(目標より行き過ぎ)
02302         else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 > target*-1)) {
02303             true_migimae_data[0]     = 0x80;
02304             true_migiusiro_data[0]   = 0x80;
02305             true_hidarimae_data[0]   = 0x80;
02306             true_hidariusiro_data[0] = 0x80;
02307         }
02308 }
02309 
02310 void left_PID(int target) {
02311 
02312         //センサ出力値の最小、最大
02313         left_migimae.setInputLimits(-2147483648,     2147483647);
02314         left_migiusiro.setInputLimits(-2147483648,   2147483647);
02315         left_hidarimae.setInputLimits(-2147483648,   2147483647);
02316         left_hidariusiro.setInputLimits(-2147483648, 2147483647);
02317 
02318         //制御量の最小、最大
02319         //左進(目標まで達していない)
02320         if((x_pulse1 < target) && (x_pulse2 < target)) {
02321             left_migimae.setOutputLimits(0xEC,     0xED);
02322             left_migiusiro.setOutputLimits(0x7A,   0x7B);
02323             left_hidarimae.setOutputLimits(0x6E,   0x6F);
02324             left_hidariusiro.setOutputLimits(0xFE, 0xFF);
02325         }
02326         //停止(目標より行き過ぎ)
02327         else if((x_pulse1 > target) && (x_pulse2 > target)) {
02328             left_migimae.setOutputLimits(0x7C,     0x83);
02329             left_migiusiro.setOutputLimits(0x7C,   0x83);
02330             left_hidarimae.setOutputLimits(0x7C,   0x83);
02331             left_hidariusiro.setOutputLimits(0x7C, 0x83);
02332         }
02333 
02334         //よくわからんやつ
02335         left_migimae.setMode(AUTO_MODE);
02336         left_migiusiro.setMode(AUTO_MODE);
02337         left_hidarimae.setMode(AUTO_MODE);
02338         left_hidariusiro.setMode(AUTO_MODE);
02339 
02340         //目標値
02341         left_migimae.setSetPoint(target);
02342         left_migiusiro.setSetPoint(target);
02343         left_hidarimae.setSetPoint(target);
02344         left_hidariusiro.setSetPoint(target);
02345 
02346         //センサ出力
02347         left_migimae.setProcessValue(x_pulse1);
02348         left_migiusiro.setProcessValue(x_pulse2);
02349         left_hidarimae.setProcessValue(x_pulse1);
02350         left_hidariusiro.setProcessValue(x_pulse2);
02351 
02352         //制御量(計算結果)
02353         migimae_data[0]      = left_migimae.compute();
02354         migiusiro_data[0]    = left_migiusiro.compute();
02355         hidarimae_data[0]    = left_hidarimae.compute();
02356         hidariusiro_data[0]  = left_hidariusiro.compute();
02357 
02358         //制御量をPWM値に変換
02359         //左進(目標まで達していない)
02360         if((x_pulse1 < target) && (x_pulse2 < target)) {
02361             true_migimae_data[0]     = migimae_data[0];
02362             true_migiusiro_data[0]   = 0x7B - migiusiro_data[0];
02363             true_hidarimae_data[0]   = 0x7B - hidarimae_data[0];
02364             true_hidariusiro_data[0] = hidariusiro_data[0];
02365         }
02366         //停止(目標より行き過ぎ)
02367         else if((x_pulse1 > target) && (x_pulse2 > target)) {
02368             true_migimae_data[0]     = 0x80;
02369             true_migiusiro_data[0]   = 0x80;
02370             true_hidarimae_data[0]   = 0x80;
02371             true_hidariusiro_data[0] = 0x80;
02372         }
02373 }
02374 
02375 void turn_right_PID(int target) {
02376 
02377         //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
02378         turn_right_migimae.setInputLimits(-2147483648,     2147483647);
02379         turn_right_migiusiro.setInputLimits(-2147483648,   2147483647);
02380         turn_right_hidarimae.setInputLimits(-2147483648,   2147483647);
02381         turn_right_hidariusiro.setInputLimits(-2147483648, 2147483647);
02382 
02383         //制御量の最小、最大
02384         //右旋回(目標に達してない)
02385         if(sum_pulse < target) {
02386             turn_right_migimae.setOutputLimits(0x10,   0x7B);
02387             turn_right_migiusiro.setOutputLimits(0x10, 0x7B);
02388             turn_right_hidarimae.setOutputLimits(0x94, 0xFF);
02389             turn_right_hidariusiro.setOutputLimits(0x94, 0xFF);
02390         }
02391         //停止(目標より行き過ぎ)
02392         else if(sum_pulse > target) {
02393             turn_right_migimae.setOutputLimits(0x7C,     0x83);
02394             turn_right_migiusiro.setOutputLimits(0x7C,   0x83);
02395             turn_right_hidarimae.setOutputLimits(0x7C,   0x83);
02396             turn_right_hidariusiro.setOutputLimits(0x7C, 0x83);
02397         }
02398 
02399         //よくわからんやつ
02400         turn_right_migimae.setMode(AUTO_MODE);
02401         turn_right_migiusiro.setMode(AUTO_MODE);
02402         turn_right_hidarimae.setMode(AUTO_MODE);
02403         turn_right_hidariusiro.setMode(AUTO_MODE);
02404 
02405         //目標値
02406         turn_right_migimae.setSetPoint(target);
02407         turn_right_migiusiro.setSetPoint(target);
02408         turn_right_hidarimae.setSetPoint(target);
02409         turn_right_hidariusiro.setSetPoint(target);
02410 
02411         //センサ出力
02412         turn_right_migimae.setProcessValue(sum_pulse);
02413         turn_right_migiusiro.setProcessValue(sum_pulse);
02414         turn_right_hidarimae.setProcessValue(sum_pulse);
02415         turn_right_hidariusiro.setProcessValue(sum_pulse);
02416 
02417         //制御量(計算結果)
02418         migimae_data[0]      = turn_right_migimae.compute();
02419         migiusiro_data[0]    = turn_right_migiusiro.compute();
02420         hidarimae_data[0]    = turn_right_hidarimae.compute();
02421         hidariusiro_data[0]  = turn_right_hidariusiro.compute();
02422 
02423         //制御量をPWM値に変換
02424         //右旋回(目標に達してない)
02425         if(sum_pulse < target) {
02426             true_migimae_data[0]     = 0x7B - migimae_data[0];
02427             true_migiusiro_data[0]   = 0x7B - migiusiro_data[0];
02428             true_hidarimae_data[0]   = hidarimae_data[0];
02429             true_hidariusiro_data[0] = hidariusiro_data[0];
02430         }
02431         //停止(目標より行き過ぎ)
02432         else if(sum_pulse > target) {
02433             true_migimae_data[0]     = 0x80;
02434             true_migiusiro_data[0]   = 0x80;
02435             true_hidarimae_data[0]   = 0x80;
02436             true_hidariusiro_data[0] = 0x80;
02437         }
02438 }
02439 
02440 void turn_left_PID(int target) {
02441 
02442         //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
02443         turn_left_migimae.setInputLimits(-2147483648,     2147483647);
02444         turn_left_migiusiro.setInputLimits(-2147483648,   2147483647);
02445         turn_left_hidarimae.setInputLimits(-2147483648,   2147483647);
02446         turn_left_hidariusiro.setInputLimits(-2147483648, 2147483647);
02447 
02448         //制御量の最小、最大
02449         //左旋回(目標に達してない)
02450         if(sum_pulse < target) {
02451             turn_left_migimae.setOutputLimits(0x94,   0xFF);
02452             turn_left_migiusiro.setOutputLimits(0x94, 0xFF);
02453             turn_left_hidarimae.setOutputLimits(0x10, 0x7B);
02454             turn_left_hidariusiro.setOutputLimits(0x10, 0x7B);
02455         }
02456         //停止(目標より行き過ぎ)
02457         else if(sum_pulse > target) {
02458             turn_left_migimae.setOutputLimits(0x7C,     0x83);
02459             turn_left_migiusiro.setOutputLimits(0x7C,   0x83);
02460             turn_left_hidarimae.setOutputLimits(0x7C,   0x83);
02461             turn_left_hidariusiro.setOutputLimits(0x7C, 0x83);
02462         }
02463 
02464         //よくわからんやつ
02465         turn_left_migimae.setMode(AUTO_MODE);
02466         turn_left_migiusiro.setMode(AUTO_MODE);
02467         turn_left_hidarimae.setMode(AUTO_MODE);
02468         turn_left_hidariusiro.setMode(AUTO_MODE);
02469 
02470         //目標値
02471         turn_left_migimae.setSetPoint(target);
02472         turn_left_migiusiro.setSetPoint(target);
02473         turn_left_hidarimae.setSetPoint(target);
02474         turn_left_hidariusiro.setSetPoint(target);
02475 
02476         //センサ出力
02477         turn_left_migimae.setProcessValue(sum_pulse);
02478         turn_left_migiusiro.setProcessValue(sum_pulse);
02479         turn_left_hidarimae.setProcessValue(sum_pulse);
02480         turn_left_hidariusiro.setProcessValue(sum_pulse);
02481 
02482         //制御量(計算結果)
02483         migimae_data[0]      = turn_left_migimae.compute();
02484         migiusiro_data[0]    = turn_left_migiusiro.compute();
02485         hidarimae_data[0]    = turn_left_hidarimae.compute();
02486         hidariusiro_data[0]  = turn_left_hidariusiro.compute();
02487 
02488         //制御量をPWM値に変換
02489         //左旋回(目標に達してない)
02490         if(sum_pulse < target) {
02491             true_migimae_data[0]     = migimae_data[0];
02492             true_migiusiro_data[0]   = migiusiro_data[0];
02493             true_hidarimae_data[0]   = 0x7B - hidarimae_data[0];
02494             true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
02495         }
02496         //左旋回(目標より行き過ぎ)
02497         else if(sum_pulse > target) {
02498             true_migimae_data[0]     = 0x80;
02499             true_migiusiro_data[0]   = 0x80;
02500             true_hidarimae_data[0]   = 0x80;
02501             true_hidariusiro_data[0] = 0x80;
02502         }
02503 }