2019年10月18日現在の最新版 -tyokudo_phaseを回収の時に見るのをやめた +送風時間をマクロで変更できるようにし、送風用のサブルーチンを作成した

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