停止時にstop関数をループさせずに、stop_flagを利用して1度だけi2c書き込みをMDに行うようにした。i2cのデータがスタックするのを防止する為(ちなみにi2cをフルで使用した際の周期は2.4[ms]であった)。

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