2019年10月02日AM11:14現在のもの(青ゾーンは変更なし)

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