1日体験入学のやつ

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 /* Sensor: encorder*4                                                   */
00006 /* -------------------------------------------------------------------  */
00007 /* blue zone is ok, added back phase                                    */
00008 /* -------------------------------------------------------------------  */
00009 #include "mbed.h"
00010 #include "math.h"
00011 #include "QEI.h"
00012 #include "PID.h"
00013 
00014 //直進補正の為の前後・左右の回転差の許容値
00015 #define wheel_difference    100
00016 
00017 #define RED     0
00018 #define BLUE    1
00019 
00020 //PID Gain of wheels(Kp, Ti, Td, control cycle)
00021 //前進
00022 PID front_migimae(4500000.0, 0.0, 0.0, 0.001);
00023 PID front_migiusiro(4500000.0, 0.0, 0.0, 0.001);
00024 PID front_hidarimae(4500000.0, 0.0, 0.0, 0.001);
00025 PID front_hidariusiro(4500000.0, 0.0, 0.0, 0.001);
00026 
00027 //後進
00028 PID back_migimae(4500000.0, 0.0, 0.0, 0.001);
00029 PID back_migiusiro(4500000.0, 0.0, 0.0, 0.001);
00030 PID back_hidarimae(4500000.0, 0.0, 0.0, 0.001);
00031 PID back_hidariusiro(4500000.0, 0.0, 0.0, 0.001);
00032 
00033 //右進
00034 PID right_migimae(6000000.0, 0.0, 0.0, 0.001);
00035 PID right_migiusiro(6000000.0, 0.0, 0.0, 0.001);
00036 PID right_hidarimae(6000000.0, 0.0, 0.0, 0.001);
00037 PID right_hidariusiro(6000000.0, 0.0, 0.0, 0.001);
00038 
00039 //左進
00040 PID left_migimae(6000000.0, 0.0, 0.0, 0.001);
00041 PID left_migiusiro(6000000.0, 0.0, 0.0, 0.001);
00042 PID left_hidarimae(6000000.0, 0.0, 0.0, 0.001);
00043 PID left_hidariusiro(6000000.0, 0.0, 0.0, 0.001);
00044 
00045 //右旋回
00046 PID turn_right_migimae(30000000.0, 0.0, 0.0, 0.001);
00047 PID turn_right_migiusiro(30000000.0, 0.0, 0.0, 0.001);
00048 PID turn_right_hidarimae(30000000.0, 0.0, 0.0, 0.001);
00049 PID turn_right_hidariusiro(30000000.0, 0.0, 0.0, 0.001);
00050 
00051 //左旋回
00052 PID turn_left_migimae(30000000.0, 0.0, 0.0, 0.001);
00053 PID turn_left_migiusiro(30000000.0, 0.0, 0.0, 0.001);
00054 PID turn_left_hidarimae(30000000.0, 0.0, 0.0, 0.001);
00055 PID turn_left_hidariusiro(30000000.0, 0.0, 0.0, 0.001);
00056 
00057 //MDとの通信ポート
00058 I2C i2c(PB_9, PB_8);  //SDA, SCL
00059 
00060 //PCとの通信ポート
00061 Serial pc(USBTX, USBRX);    //TX, RX
00062 
00063 //特小モジュールとの通信ポート
00064 Serial pic(A0, A1);
00065 
00066 //リミットスイッチ基板との通信ポート
00067 Serial limit_serial(PC_12, PD_2);
00068 
00069 //12V停止信号ピン
00070 DigitalOut emergency(D11);
00071 
00072 DigitalOut USR_LED1(PB_7);
00073 //DigitalOut USR_LED2(PC_13);
00074 DigitalOut USR_LED3(PC_2);
00075 DigitalOut USR_LED4(PC_3);
00076 DigitalOut  GREEN_LED(D8);
00077 DigitalOut  RED_LED(D10);
00078 
00079 //遠隔非常停止ユニットLED
00080 AnalogOut myled(A2);
00081 
00082 DigitalIn start_switch(PB_12);
00083 DigitalIn USR_SWITCH(PC_13);
00084 DigitalIn zone_switch(PC_10);
00085 
00086 QEI wheel_x1(PA_8 , PA_6 , NC, 624);
00087 QEI wheel_x2(PB_14, PB_13, NC, 624);
00088 QEI wheel_y1(PB_1 , PB_15, NC, 624);
00089 QEI wheel_y2(PA_12, PA_11, NC, 624);
00090 QEI arm_enc(PB_5,   PB_4 , NC, 624);
00091 
00092 //移動後n秒停止タイマー
00093 Timer counter;
00094 
00095 //エンコーダ値格納変数
00096 int x_pulse1, x_pulse2, y_pulse1, y_pulse2;
00097 
00098 //操作の段階変数
00099 unsigned int phase = 0;
00100 int kaisyu_phase = 0;
00101 int tyokudo_phase = 0;
00102 unsigned int start_zone = 1;
00103 bool zone = RED;
00104 
00105 //i2c送信データ変数
00106 char init_send_data[1];
00107 char migimae_data[1], migiusiro_data[1], hidarimae_data[1], hidariusiro_data[1];
00108 char true_migimae_data[1], true_migiusiro_data[1], true_hidarimae_data[1], true_hidariusiro_data[1];
00109 char arm_motor[1], drop_motor[1];
00110 char fan_data[1];
00111 char servo_data[1];
00112 char right_arm_data[1], left_arm_data[1];
00113 
00114 //非常停止関連変数
00115 char RDATA;
00116 char baff;
00117 int flug = 0;
00118 
00119 //リミット基板からの受信データ
00120 int limit_data = 0;
00121 int upper_limit_data = 0;
00122 int lower_limit_data = 0;
00123 
00124 //各辺のスイッチが押されたかのフラグ
00125 //前部が壁に当たっているか
00126 int front_limit = 0;
00127 //右部が壁にあたあっているか
00128 int right_limit = 0;
00129 //後部が壁に当たっているか
00130 int back_limit = 0;
00131 //回収機構の下限(引っ込めてるほう)
00132 bool kaisyu_mae_limit = 0;
00133 
00134 bool kaisyu_usiro_limit = 0;
00135 
00136 //右腕の下限
00137 bool right_arm_lower_limit = 0;
00138 //右腕の上限
00139 bool right_arm_upper_limit = 0;
00140 //左腕の下限
00141 bool left_arm_lower_limit = 0;
00142 //左腕の上限
00143 bool left_arm_upper_limit = 0;
00144 //吐き出し機構の上限
00145 bool tyokudo_mae_limit = 0;
00146 //吐き出し機構の下限
00147 bool tyokudo_usiro_limit = 0;
00148 
00149 int masked_lower_front_limit_data     = 0xFF;
00150 int masked_lower_back_limit_data      = 0xFF;
00151 int masked_lower_right_limit_data     = 0xFF;
00152 int masked_kaisyu_mae_limit_data      = 0xFF;
00153 int masked_kaisyu_usiro_limit_data    = 0xFF;
00154 int masked_right_arm_lower_limit_data = 0xFF;
00155 int masked_right_arm_upper_limit_data = 0xFF;
00156 int masked_left_arm_lower_limit_data  = 0xFF;
00157 int masked_left_arm_upper_limit_data  = 0xFF;
00158 int masked_tyokudo_mae_limit_data     = 0xFF;
00159 int masked_tyokudo_usiro_limit_data   = 0xFF;
00160 
00161 //関数のプロトタイプ宣言
00162 void init(void);
00163 void init_send(void);
00164 void get(void);
00165 void get_pulses(void);
00166 void print_pulses(void);
00167 void get_emergency(void);
00168 void read_limit(void);
00169 void wheel_reset(void);
00170 void kaisyu(int pulse, int next_phase);
00171 void tyokudo(int pulse, int next_phase);
00172 void arm_up(int next_phase);
00173 void front(int target);
00174 void back(int target);
00175 void right(int target);
00176 void left(int target);
00177 void turn_right(int target);
00178 void turn_left(int target);
00179 void stop(void);
00180 void front_PID(int target);
00181 void back_PID(int target);
00182 void right_PID(int target);
00183 void left_PID(int target);
00184 void turn_right_PID(int target);
00185 void turn_left_PID(int target);
00186 
00187 int main(void) {
00188 
00189     init();
00190     init_send();
00191 
00192     //とりあえず(後で消してね)
00193     //zone = BLUE;
00194     //phase = 16;
00195     //phase = 23;
00196     phase = 30;
00197     
00198     //起動時にゾーンを読んでからループに入る(試合中誤ってスイッチ押すのを防止)
00199     while(1) {
00200         if(zone_switch == 0) {
00201             zone = BLUE;
00202         } else {
00203             zone = RED;
00204         }
00205         break;
00206     }
00207         
00208     while(1) {
00209 
00210         get_pulses();
00211         print_pulses();
00212         get_emergency();
00213         read_limit();
00214                 
00215         //move_servo_with_using_onboard-switch
00216         if(USR_SWITCH == 0) {
00217             servo_data[0] = 0x03;
00218             i2c.write(0x30, servo_data, 1);
00219         } else {
00220             servo_data[0] = 0x04;
00221             i2c.write(0x30, servo_data, 1);
00222         }
00223         
00224         if(start_switch == 1) {
00225             phase = 23;
00226         }
00227                 
00228         //青ゾーン
00229         if(zone == BLUE) {
00230             GREEN_LED = 1;
00231             RED_LED = 0;
00232 
00233             switch(phase) {
00234 
00235                 //スタート位置へセット
00236                 case 0:
00237                     //リミットが洗濯物台に触れているか
00238                     if(right_limit == 3) {
00239                         USR_LED1 = 1;
00240                         //スタートスイッチが押されたか
00241                         if(start_switch == 1) {
00242                             wheel_reset();
00243                             phase = 1;
00244                         }
00245                     } else {
00246                         USR_LED1 = 0;
00247                     }
00248                     break;
00249 
00250                 //回収
00251                 case 1:          
00252                     kaisyu(arm_enc.getPulses(), 2);
00253                     servo_data[0] = 0x03;
00254                     i2c.write(0x30, servo_data, 1);
00255                     break;
00256 
00257                 //1秒停止
00258                 case 2:
00259                     stop();
00260                     servo_data[0] = 0x04;
00261                     i2c.write(0x30, servo_data, 1);
00262                     counter.start();
00263                     if(counter.read() > 1.0f) {
00264                         phase = 3;
00265                         wheel_reset();
00266                     }
00267                     break;
00268 
00269                 //左移動
00270                 case 3:
00271                     counter.reset();
00272                     left(10000);
00273                     if((x_pulse1 > 10000) && (x_pulse2 > 10000)) {
00274                         phase = 4;
00275                     }
00276                     break;
00277 
00278                 //1秒停止
00279                 case 4:
00280                     stop();
00281                     counter.start();
00282                     if(counter.read() > 1.0f) {
00283                         phase = 5;
00284                         wheel_reset();
00285                     }
00286                     break;
00287 
00288                 //右旋回(180°)
00289                 case 5:
00290                     counter.reset();
00291                     turn_right(518);
00292                     if(x_pulse2 > 518) {
00293                         phase = 6;
00294                     }
00295                     break;
00296 
00297                 //1秒停止
00298                 case 6:
00299                     stop();
00300                     counter.start();
00301                     if(counter.read() > 1.0f) {
00302                         phase = 7;
00303                         wheel_reset();
00304                     }
00305                     break;
00306 
00307                 //ちょっくら右移動
00308                 case 7:
00309                     counter.reset();
00310                     right(-2000);
00311                     
00312                     if(right_limit == 3) {
00313                         phase = 8;
00314                     }
00315                     break;
00316 
00317                 //1秒停止
00318                 case 8:
00319                     stop();
00320                     counter.start();
00321                     if(counter.read() > 1.0f) {
00322                         phase = 9;
00323                         wheel_reset();
00324                     }
00325                     break;
00326 
00327                 //排出
00328                 case 9:
00329                     counter.reset();
00330                     tyokudo(arm_enc.getPulses(), 10);
00331                     break;
00332 
00333                 //1秒停止
00334                 case 10:
00335                     stop();
00336                     counter.start();
00337                     if(counter.read() > 1.0f) {
00338                         phase = 11;
00339                         wheel_reset();
00340                     }
00341                     break;
00342 
00343                 //前進
00344                 case 11:
00345                     counter.reset();
00346                     front(3500);
00347                     if((y_pulse1 > 3500) && (y_pulse2 > 3500)) {
00348                         phase = 12;
00349                     }
00350                     break;
00351 
00352                 //1秒停止
00353                 case 12:
00354                     stop();
00355                     counter.start();
00356                     if(counter.read() > 1.0f) {
00357                         phase = 13;
00358                         wheel_reset();
00359                     }
00360                     break;
00361 
00362                 //右移動
00363                 case 13:
00364                     counter.reset();
00365                     right(-4000);
00366                     if(right_limit == 3) {
00367                         phase = 14;
00368                     }
00369                     break;
00370                     
00371                 //1秒停止
00372                 case 14:
00373                     stop();
00374                     counter.start();
00375                     if(counter.read() > 1.0f) {
00376                         phase = 16;
00377                         wheel_reset();
00378                     }
00379                     break;
00380 
00381                 /*
00382                 //後進
00383                 case 15:
00384                     counter.reset();
00385                     back(-1000);
00386 
00387                     if(back_limit == 3) {
00388                         phase = 16;
00389                     }
00390                 */
00391                    
00392                 //シーツ装填
00393                 case 16:
00394                     if(start_switch == 1) {
00395                         wheel_reset();
00396                         phase = 17;
00397                     } else {
00398                         stop();
00399                     }
00400                     break;
00401 
00402                 //竿のラインまで前進
00403                 case 17:
00404                     counter.reset();
00405                     front(22000);
00406                     if((y_pulse1 > 22000) && (y_pulse2 > 22000)) {
00407                         phase = 18;
00408                     }
00409                     break;
00410 
00411                 //1秒停止
00412                 case 18:
00413                     stop();
00414                     counter.start();
00415                     if(counter.read() > 1.0f) {
00416                         phase = 19;
00417                         wheel_reset();
00418                     }
00419                     break;
00420 
00421                 //掛けるところまで左移動
00422                 case 19:
00423                     counter.reset();
00424                     left(10000);
00425                     if((x_pulse1 > 10000) && (x_pulse2 > 10000)) {
00426                         phase = 20;
00427                     }
00428                     break;
00429 
00430                 //1秒停止
00431                 case 20:
00432                     stop();
00433                     counter.start();
00434                     if(counter.read() > 1.0f) {
00435                         phase = 21;
00436                         wheel_reset();
00437                     }
00438                     break;
00439 
00440                 //妨害防止の右旋回
00441                 case 21:
00442                     counter.reset();
00443                     turn_right(280);
00444                     if(x_pulse2 > 280) {
00445                         phase = 22;
00446                     }
00447                     break;
00448 
00449                 //1秒停止
00450                 case 22:
00451                     stop();
00452                     counter.start();
00453                     if(counter.read() > 1.0f) {
00454                         phase = 23;
00455                         wheel_reset();
00456                     }
00457                     break;
00458 
00459                 //カウンターリセット
00460                 case 23:
00461                     counter.reset();
00462                     counter.start();
00463                     phase = 24;
00464                     
00465                 //アームアップ
00466                 case 24:
00467                     stop();
00468                     
00469                     if(counter.read() < 3.0f) {
00470                         right_arm_data[0] = 0xFF;
00471                         left_arm_data[0]  = 0xFF;
00472                         i2c.write(0x22, right_arm_data, 1);
00473                         i2c.write(0x24, left_arm_data, 1);
00474                         wait_us(20);
00475 
00476                     } else {
00477                         arm_up(25);
00478                     }
00479                     break;
00480                     
00481                 //カウンターリセット
00482                 case 25:
00483                     counter.reset();
00484                     phase = 26;
00485                 
00486                 //シーツを掛ける
00487                 case 26:
00488                     counter.start();
00489                     
00490                     //1秒間ファン送風
00491                     if(counter.read() <= 1.0f) {
00492                         fan_data[0] = 0xFF;
00493                         i2c.write(0x26, fan_data, 1);
00494                         i2c.write(0x28, fan_data, 1);
00495                         servo_data[0] = 0x04;
00496                         i2c.write(0x30, servo_data, 1);
00497                     }
00498                     //1~3秒の間でサーボを話す
00499                     else if((counter.read() > 1.0f) && (counter.read() <= 3.0f)) {
00500                         fan_data[0] = 0xFF;
00501                         i2c.write(0x26, fan_data, 1);
00502                         i2c.write(0x28, fan_data, 1);
00503                         servo_data[0] = 0x03;
00504                         i2c.write(0x30, servo_data, 1);
00505                     }
00506                     //3秒過ぎたら終わり
00507                     else if(counter.read() > 3.0f) {
00508                         fan_data[0] = 0x80;
00509                         i2c.write(0x26, fan_data, 1);
00510                         i2c.write(0x28, fan_data, 1);
00511                         servo_data[0] = 0x04;
00512                         i2c.write(0x30, servo_data, 1);
00513                         phase = 27;
00514                     }
00515                     break;
00516 
00517                 //終了っ!(守衛さん風)
00518                 case 27:
00519                     //駆動系統OFF
00520                     emergency = 0;
00521                     break;
00522 
00523                 default:
00524                     //駆動系統OFF
00525                     emergency = 0;
00526                     break;
00527             }
00528         }
00529         
00530         //REDゾーン
00531         else if(zone == RED) {
00532             GREEN_LED = 0;
00533             RED_LED = 1;
00534         }
00535     }
00536 }
00537 
00538 void init(void) {
00539 
00540     //通信ボーレートの設定
00541     pc.baud(460800);
00542 
00543     limit_serial.baud(115200);
00544 
00545     start_switch.mode(PullUp);
00546     zone_switch.mode(PullDown);
00547 
00548     //非常停止関連
00549     pic.baud(19200);
00550     pic.format(8, Serial::None, 1);
00551     pic.attach(get, Serial::RxIrq);
00552 
00553     x_pulse1 = 0;   x_pulse2 = 0;   y_pulse1 = 0;   y_pulse2 = 0;
00554     migimae_data[0] = 0x80; migiusiro_data[0] = 0x80;   hidarimae_data[0] = 0x80;   hidariusiro_data[0] = 0x80;
00555     true_migimae_data[0] = 0x80;    true_migiusiro_data[0] = 0x80;  true_hidarimae_data[0] = 0x80;  true_hidariusiro_data[0] = 0x80;
00556     fan_data[0] = 0x80;
00557     servo_data[0] = 0x80;
00558     arm_motor[0] = 0x80;    drop_motor[0] = 0x80;
00559     right_arm_data[0] = 0x80;   left_arm_data[0] = 0x80;
00560 }
00561 
00562 void init_send(void) {
00563 
00564     init_send_data[0] = 0x80;
00565     i2c.write(0x10, init_send_data, 1);
00566     i2c.write(0x12, init_send_data, 1);
00567     i2c.write(0x14, init_send_data, 1);
00568     i2c.write(0x16, init_send_data, 1);
00569     i2c.write(0x18, init_send_data, 1);
00570     i2c.write(0x20, init_send_data, 1);
00571     i2c.write(0x22, init_send_data, 1);
00572     i2c.write(0x24, init_send_data, 1);
00573     i2c.write(0x30, init_send_data, 1);
00574     wait(0.1);
00575 }
00576 
00577 void get(void) {
00578 
00579     baff = pic.getc();
00580 
00581     for(; flug; flug--)
00582         RDATA = baff;
00583 
00584     if(baff == ':')
00585         flug = 1;
00586 }
00587 
00588 void get_pulses(void) {
00589 
00590         x_pulse1 = wheel_x1.getPulses();
00591         x_pulse2 = wheel_x2.getPulses();
00592         y_pulse1 = wheel_y1.getPulses();
00593         y_pulse2 = wheel_y2.getPulses();
00594 }
00595 
00596 void print_pulses(void) {
00597         
00598         pc.printf("phase: %d\n\r", phase);
00599         //pc.printf("r: %d, l: %d, %d\n\r", right_arm_upper_limit, left_arm_upper_limit, phase);
00600         //pc.printf("%r: %x, l: %x\n\r", right_arm_data[0], left_arm_data[0]);
00601         //pc.printf("limit: 0x%x, upper: 0x%x, lower: 0x%x\n\r", limit_data, upper_limit_data, lower_limit_data);
00602         //pc.printf("x1: %d, x2: %d, y1: %d, y2: %d, phase: %d\n\r", x_pulse1, x_pulse2, y_pulse1, y_pulse2, phase);
00603         //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);
00604         //pc.printf("RF: %x, RB: %x, LF: %x, LB: %x, phase: %d\n\r", migimae_data[0], migiusiro_data[0], hidarimae_data[0], hidariusiro_data[0], phase);
00605 
00606 }
00607 
00608 void get_emergency(void) {
00609 
00610     if(RDATA == '1') {
00611         myled = 1;
00612         emergency = 1;
00613     }
00614     else if(RDATA == '9'){
00615         myled = 0.2;
00616         emergency = 0;
00617     }
00618 }
00619 
00620 void read_limit(void) {
00621 
00622         limit_data = limit_serial.getc();
00623 
00624         //上位1bitが1ならば下のリミットのデータだと判断
00625         if((limit_data & 0b10000000) == 0b10000000) {
00626             lower_limit_data = limit_data;
00627 
00628         //上位1bitが0ならば上のリミットのデータだと判断
00629         } else {
00630             upper_limit_data = limit_data;
00631         }
00632 
00633         //下リミット基板からのデータのマスク処理
00634         masked_lower_front_limit_data = lower_limit_data & 0b00000011;
00635         masked_lower_back_limit_data  = lower_limit_data & 0b00001100;
00636         masked_lower_right_limit_data = lower_limit_data & 0b00110000;
00637         masked_kaisyu_mae_limit_data      = lower_limit_data & 0b01000000;
00638 
00639         //上リミット基板からのデータのマスク処理
00640         //masked_right_arm_lower_limit_data = upper_limit_data & 0b00000001;
00641         masked_kaisyu_usiro_limit_data    = upper_limit_data & 0b00000001;
00642         masked_right_arm_upper_limit_data = upper_limit_data & 0b00000010;
00643         masked_left_arm_lower_limit_data  = upper_limit_data & 0b00000100;
00644         masked_left_arm_upper_limit_data  = upper_limit_data & 0b00001000;
00645         masked_tyokudo_mae_limit_data     = upper_limit_data & 0b00010000;
00646         masked_tyokudo_usiro_limit_data   = upper_limit_data & 0b00100000;
00647 
00648         //前部リミット
00649         switch(masked_lower_front_limit_data) {
00650             //両方押された
00651             case 0x00:
00652                 front_limit = 3;
00653                 break;
00654             //右が押された
00655             case 0b00000010:
00656                 front_limit = 1;
00657                 break;
00658             //左が押された
00659             case 0b00000001:
00660                 front_limit = 2;
00661                 break;
00662             default:
00663                 front_limit = 0;
00664                 break;
00665         }
00666 
00667         //後部リミット
00668         switch(masked_lower_back_limit_data) {
00669             //両方押された
00670             case 0x00:
00671                 back_limit = 3;
00672                 break;
00673             //右が押された
00674             case 0b00001000:
00675                 back_limit = 1;
00676                 break;
00677             //左が押された
00678             case 0b00000100:
00679                 back_limit = 2;
00680                 break;
00681             default:
00682                 back_limit = 0;
00683                 break;
00684         }
00685 
00686         //右部リミット
00687         switch(masked_lower_right_limit_data) {
00688             //両方押された
00689             case 0x00:
00690                 right_limit = 3;
00691                 break;
00692             //右が押された
00693             case 0b00100000:
00694                 right_limit = 1;
00695                 break;
00696             //左が押された
00697             case 0b00010000:
00698                 right_limit = 2;
00699                 break;
00700             default:
00701                 right_limit = 0;
00702                 break;
00703         }
00704 
00705         //回収機構リミット
00706         switch(masked_kaisyu_mae_limit_data) {
00707             //押された
00708             case 0b00000000:
00709                 kaisyu_mae_limit = 1;
00710                 break;
00711             case 0b01000000:
00712                   kaisyu_mae_limit = 0;
00713                 break;
00714             default:
00715                 kaisyu_mae_limit = 0;
00716                 break;
00717         }
00718 
00719         //右腕下部リミット
00720         /*
00721         switch(masked_right_arm_lower_limit_data) {
00722             //押された
00723             case 0b00000000:
00724                 right_arm_lower_limit = 1;
00725                 break;
00726             case 0b00000001:
00727                 right_arm_lower_limit = 0;
00728                 break;
00729             default:
00730                 right_arm_lower_limit = 0;
00731                 break;
00732         }
00733         */
00734         
00735         //回収後リミット
00736         switch(masked_kaisyu_usiro_limit_data) {
00737             case 0b00000000:
00738                 kaisyu_usiro_limit = 1;
00739                 break;
00740             case 0b00000001:
00741                 kaisyu_usiro_limit = 0;
00742                 break;
00743             default:
00744                 kaisyu_usiro_limit = 0;
00745                 break;
00746         }
00747         
00748         //右腕上部リミット
00749         switch(masked_right_arm_upper_limit_data) {
00750             //押された
00751             case 0b00000000:
00752                 right_arm_upper_limit = 1;
00753                 break;
00754             case 0b00000010:
00755                 right_arm_upper_limit = 0;
00756                 break;
00757             default:
00758                 right_arm_upper_limit = 0;
00759                 break;
00760         }
00761 
00762         //左腕下部リミット
00763         switch(masked_left_arm_lower_limit_data) {
00764             //押された
00765             case 0b00000000:
00766                 left_arm_lower_limit = 1;
00767                 break;
00768             case 0b00000100:
00769                 left_arm_lower_limit = 0;
00770                 break;
00771             default:
00772                 left_arm_lower_limit = 0;
00773                 break;
00774         }
00775 
00776         //左腕上部リミット
00777         switch(masked_left_arm_upper_limit_data) {
00778             //押された
00779             case 0b00000000:
00780                 left_arm_upper_limit = 1;
00781                 break;
00782             case 0b00001000:
00783                 left_arm_upper_limit = 0;
00784                 break;
00785             default:
00786                 left_arm_upper_limit = 0;
00787                 break;
00788         }
00789 
00790         //直動の前
00791         switch(masked_tyokudo_mae_limit_data) {
00792             //押された
00793             case 0b00000000:
00794                 tyokudo_mae_limit = 1;
00795                 break;
00796             case 0b00010000:
00797                 tyokudo_mae_limit = 0;
00798                 break;
00799             default:
00800                 tyokudo_mae_limit = 0;
00801                 break;
00802         }
00803 
00804         //直動の後
00805         switch(masked_tyokudo_usiro_limit_data) {
00806             //押された
00807             case 0b00000000:
00808                 tyokudo_usiro_limit = 1;
00809                 break;
00810             case 0b00100000:
00811                 tyokudo_usiro_limit = 0;
00812                 break;
00813             default:
00814                 tyokudo_usiro_limit = 0;
00815                 break;
00816         }
00817 }
00818 
00819 void wheel_reset(void) {
00820 
00821     wheel_x1.reset();
00822     wheel_x2.reset();
00823     wheel_y1.reset();
00824     wheel_y2.reset();
00825 }
00826 
00827 void kaisyu(int pulse, int next_phase) {
00828 
00829     switch (kaisyu_phase) {
00830 
00831         case 0:
00832             //前進->減速
00833             //3000pulseまで高速前進
00834             if(pulse < 3000) {
00835                 arm_motor[0] = 0xFF;
00836                 //kaisyu_phase = 1;
00837             }
00838 
00839             //3000pulse超えたら低速前進
00840             else if(pulse >= 3000) {
00841                 arm_motor[0] = 0xB3;
00842                 kaisyu_phase = 1;
00843             }
00844             break;
00845 
00846         case 1:
00847             USR_LED3 = 1;
00848             //前進->停止->後進
00849             //3600pulseまで低速前進
00850             if(pulse < 3600) {
00851                 arm_motor[0] = 0xB3;
00852                 //kaisyu_phase = 2;
00853             }
00854 
00855             //3600pulse超えたら停止
00856             else if(pulse >= 3600) {
00857                 arm_motor[0] = 0x80;
00858 
00859                 //1秒待ってから引っ込める
00860                 counter.start();
00861                 if(counter.read() > 1.0f) {
00862                     kaisyu_phase = 2;
00863                 }
00864             }
00865             //後ろのリミットが押されたら強制停止
00866             if(kaisyu_usiro_limit == 1) {
00867                 arm_motor[0] = 0x80;
00868             }
00869             break;
00870 
00871         case 2:
00872             //後進->減速
00873             //500pulseまで高速後進
00874             counter.reset();
00875             if(pulse > 500) {
00876                 arm_motor[0] = 0x00;
00877                 //kaisyu_phase = 3;
00878 
00879             }
00880             //500pulse以下になったら低速後進
00881             else if(pulse <= 500) {
00882                 arm_motor[0] = 0x4C;
00883                 kaisyu_phase = 3;
00884             }
00885             break;
00886 
00887         case 3:
00888             //後進->停止
00889             //リミット押されるまで低速後進
00890             if(pulse <= 500) {
00891                 arm_motor[0] = 0x4C;
00892                 //kaisyu_phase = 4;
00893             }
00894 
00895             //リミット押されたら停止
00896             if(kaisyu_mae_limit == 1) {
00897                 arm_motor[0] = 0x80;
00898                 kaisyu_phase = 4;
00899                 phase = next_phase;
00900             }
00901             break;
00902 
00903         default:
00904             arm_motor[0] = 0x80;
00905             break;
00906     }
00907     
00908     //回収MDへ書き込み
00909     i2c.write(0x18, arm_motor, 1);
00910 }
00911 
00912 void tyokudo(int pulse, int next_phase) {
00913 
00914     switch(tyokudo_phase) {
00915 
00916         case 0:
00917             //前進->減速
00918 
00919             /*   エンコーダー読まずにリミットだけ(修正必須)   */
00920             //3600pulseより大きい&直堂前リミットが押されたら次のphaseへ移行
00921             if(tyokudo_mae_limit == 0) {
00922                 //2000pulseまで高速前進
00923                 if(pulse < 2000) {
00924                     arm_motor[0] = 0xC0;
00925                     drop_motor[0] = 0xE6;
00926                 }
00927                 //2000pulse以上で低速前進
00928                 else if(pulse >= 2000) {
00929                     arm_motor[0] = 0xC0;
00930                     drop_motor[0] = 0xE6;
00931                 }
00932                 //パルスが3600を終えたらアームのみ強制停止
00933                 else if(pulse > 3600) {
00934                     arm_motor[0] = 0x80;
00935                     drop_motor[0] = 0xE6;
00936                 }
00937                 
00938                 //後ろのリミットが押されたら強制停止
00939                 if(kaisyu_usiro_limit == 1) {
00940                     arm_motor[0] = 0x80;
00941                 }
00942             }
00943 
00944             //直動の前リミットが押されたら
00945             else if(tyokudo_mae_limit == 1) {
00946                 //高速後進
00947                 arm_motor[0] = 0x40;
00948                 drop_motor[0] = 0x00;
00949                 tyokudo_phase = 1;
00950             }
00951             break;
00952 
00953         case 1:
00954             //後進->減速
00955             //リミットが押されたら強制停止
00956             if(tyokudo_usiro_limit == 1) {
00957                 arm_motor[0] = 0x80;
00958                 drop_motor[0] = 0x80;
00959                 tyokudo_phase = 2;
00960                 phase = next_phase;
00961             }
00962             break;
00963             
00964         default:
00965             arm_motor[0] = 0x80;
00966             drop_motor[0] = 0x80;
00967             break;
00968     }
00969     
00970     i2c.write(0x18, arm_motor,  1);
00971     i2c.write(0x20, drop_motor, 1);
00972 }
00973 
00974 void arm_up(int next_phase) {
00975     
00976     //両腕、上限リミットが押されてなかったら上昇
00977     if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 0)) {
00978         right_arm_data[0] = 0xFF;   left_arm_data[0] = 0xFF;
00979     }
00980     //右腕のみリミットが押されたら左腕のみ上昇
00981     else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 0)) {
00982         right_arm_data[0] = 0x80;   left_arm_data[0] = 0xFF;
00983     }
00984     //左腕のみリミットが押されたら右腕のみ上昇
00985     else if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 1)) {
00986         right_arm_data[0] = 0xFF;   left_arm_data[0] = 0x80;
00987     }
00988     //両腕、上限リミットが押されたら停止
00989     else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 1)) {
00990         right_arm_data[0] = 0x80;   left_arm_data[0] = 0x80;
00991         phase = next_phase;
00992     }
00993     
00994     i2c.write(0x22, right_arm_data, 1);
00995     i2c.write(0x24, left_arm_data, 1);
00996     wait_us(20);
00997 }
00998 
00999 void front(int target) {
01000 
01001         front_PID(target);
01002         i2c.write(0x10, true_migimae_data,     1, false);
01003         i2c.write(0x12, true_migiusiro_data,   1, false);
01004         i2c.write(0x14, true_hidarimae_data,   1, false);
01005         i2c.write(0x16, true_hidariusiro_data, 1, false);
01006         wait_us(20);
01007 }
01008 
01009 void back(int target) {
01010 
01011         back_PID(target);
01012         i2c.write(0x10, true_migimae_data,     1, false);
01013         i2c.write(0x12, true_migiusiro_data,   1, false);
01014         i2c.write(0x14, true_hidarimae_data,   1, false);
01015         i2c.write(0x16, true_hidariusiro_data, 1, false);
01016         wait_us(20);
01017 }
01018 
01019 void right(int target) {
01020 
01021         right_PID(target);
01022         i2c.write(0x10, true_migimae_data,     1, false);
01023         i2c.write(0x12, true_migiusiro_data,   1, false);
01024         i2c.write(0x14, true_hidarimae_data,   1, false);
01025         i2c.write(0x16, true_hidariusiro_data, 1, false);
01026         wait_us(20);
01027 }
01028 
01029 void left(int target) {
01030 
01031         left_PID(target);
01032         i2c.write(0x10, true_migimae_data,     1, false);
01033         i2c.write(0x12, true_migiusiro_data,   1, false);
01034         i2c.write(0x14, true_hidarimae_data,   1, false);
01035         i2c.write(0x16, true_hidariusiro_data, 1, false);
01036         wait_us(20);
01037 }
01038 
01039 void turn_right(int target) {
01040 
01041         turn_right_PID(target);
01042         i2c.write(0x10, true_migimae_data,     1, false);
01043         i2c.write(0x12, true_migiusiro_data,   1, false);
01044         i2c.write(0x14, true_hidarimae_data,   1, false);
01045         i2c.write(0x16, true_hidariusiro_data, 1, false);
01046         wait_us(20);
01047 }
01048 
01049 void turn_left(int target) {
01050 
01051         turn_left_PID(target);
01052         i2c.write(0x10, true_migimae_data,     1, false);
01053         i2c.write(0x12, true_migiusiro_data,   1, false);
01054         i2c.write(0x14, true_hidarimae_data,   1, false);
01055         i2c.write(0x16, true_hidariusiro_data, 1, false);
01056         wait_us(20);
01057 }
01058 
01059 void stop(void) {
01060 
01061         true_migimae_data[0]     = 0x80;
01062         true_migiusiro_data[0]   = 0x80;
01063         true_hidarimae_data[0]   = 0x80;
01064         true_hidariusiro_data[0] = 0x80;
01065 
01066         i2c.write(0x10, true_migimae_data,     1, false);
01067         i2c.write(0x12, true_migiusiro_data,   1, false);
01068         i2c.write(0x14, true_hidarimae_data,   1, false);
01069         i2c.write(0x16, true_hidariusiro_data, 1, false);
01070         wait_us(20);
01071 }
01072 
01073 void front_PID(int target) {
01074 
01075         //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
01076         front_migimae.setInputLimits(-2147483648,     2147483647);
01077         front_migiusiro.setInputLimits(-2147483648,   2147483647);
01078         front_hidarimae.setInputLimits(-2147483648,   2147483647);
01079         front_hidariusiro.setInputLimits(-2147483648, 2147483647);
01080 
01081         //制御量の最小、最大
01082         //正転(目標に達してない)
01083         if((y_pulse1 < target) && (y_pulse2 < target)) {
01084             front_migimae.setOutputLimits(0x84,     0xF7);
01085             front_migiusiro.setOutputLimits(0x84,   0xF7);
01086             //front_migimae.setOutputLimits(0x84,     0xFF);
01087             //front_migiusiro.setOutputLimits(0x84,   0xFF);
01088             front_hidarimae.setOutputLimits(0x84,   0xFF);
01089             front_hidariusiro.setOutputLimits(0x84, 0xFF);
01090         }
01091         /*
01092         //左側が前に出ちゃった♥(右側だけ回して左側は停止)
01093         else if((y_pulse1 + wheel_difference) < y_pulse2) {
01094             front_migimae.setOutputLimits(0x84,     0xFF);
01095             front_migiusiro.setOutputLimits(0x84,   0xFF);
01096             front_hidarimae.setOutputLimits(0x7C,   0x83);
01097             front_hidariusiro.setOutputLimits(0x7C, 0x83);
01098         }
01099         //右側が前に出ちゃった♥(左側だけ回して右側は停止)
01100         else if(y_pulse1 > (y_pulse2 + wheel_difference)) {
01101             front_migimae.setOutputLimits(0x7C,     0x83);
01102             front_migiusiro.setOutputLimits(0x7C,   0x83);
01103             front_hidarimae.setOutputLimits(0x84,   0xFF);
01104             front_hidariusiro.setOutputLimits(0x84, 0xFF);
01105         }
01106         */
01107         //停止(目標より行き過ぎ)
01108         else if((y_pulse1 > target) && (y_pulse2 > target)) {
01109             front_migimae.setOutputLimits(0x7C,     0x83);
01110             front_migiusiro.setOutputLimits(0x7C,   0x83);
01111             front_hidarimae.setOutputLimits(0x7C,   0x83);
01112             front_hidariusiro.setOutputLimits(0x7C, 0x83);
01113         }
01114 
01115         //よくわからんやつ
01116         front_migimae.setMode(AUTO_MODE);
01117         front_migiusiro.setMode(AUTO_MODE);
01118         front_hidarimae.setMode(AUTO_MODE);
01119         front_hidariusiro.setMode(AUTO_MODE);
01120 
01121         //目標値
01122         front_migimae.setSetPoint(target);
01123         front_migiusiro.setSetPoint(target);
01124         front_hidarimae.setSetPoint(target);
01125         front_hidariusiro.setSetPoint(target);
01126 
01127         //センサ出力
01128         front_migimae.setProcessValue(y_pulse1);
01129         front_migiusiro.setProcessValue(y_pulse1);
01130         front_hidarimae.setProcessValue(y_pulse2);
01131         front_hidariusiro.setProcessValue(y_pulse2);
01132 
01133         //制御量(計算結果)
01134         migimae_data[0]      = front_migimae.compute();
01135         migiusiro_data[0]    = front_migiusiro.compute();
01136         hidarimae_data[0]    = front_hidarimae.compute();
01137         hidariusiro_data[0]  = front_hidariusiro.compute();
01138 
01139         //制御量をPWM値に変換
01140         //正転(目標に達してない)
01141         if((y_pulse1 < target) && (y_pulse2 < target)) {
01142             true_migimae_data[0]     = migimae_data[0];
01143             true_migiusiro_data[0]   = migiusiro_data[0];
01144             true_hidarimae_data[0]   = hidarimae_data[0];
01145             true_hidariusiro_data[0] = hidariusiro_data[0];
01146         }
01147         /*
01148         //左側が前に出ちゃった♥(右側だけ回して左側は停止)
01149         else if((y_pulse1 + wheel_difference) < y_pulse2) {
01150             true_migimae_data[0]     = migimae_data[0];
01151             true_migiusiro_data[0]   = migiusiro_data[0];
01152             true_hidarimae_data[0]   = 0x80;
01153             true_hidariusiro_data[0] = 0x80;
01154         }
01155         //右側が前に出ちゃった♥(左側だけ回して右側は停止)
01156         else if(y_pulse1 > (y_pulse2 + wheel_difference)) {
01157             true_migimae_data[0]     = 0x80;
01158             true_migiusiro_data[0]   = 0x80;
01159             true_hidarimae_data[0]   = hidarimae_data[0];
01160             true_hidariusiro_data[0] = hidariusiro_data[0];
01161         }
01162         */
01163         //停止(目標より行き過ぎ)
01164         else if((y_pulse1 > target) && (y_pulse2 > target)) {
01165             true_migimae_data[0]     = 0x80;
01166             true_migiusiro_data[0]   = 0x80;
01167             true_hidarimae_data[0]   = 0x80;
01168             true_hidariusiro_data[0] = 0x80;
01169         }
01170 }
01171 
01172 void back_PID(int target) {
01173 
01174         //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
01175         back_migimae.setInputLimits(-2147483648,     2147483647);
01176         back_migiusiro.setInputLimits(-2147483648,   2147483647);
01177         back_hidarimae.setInputLimits(-2147483648,   2147483647);
01178         back_hidariusiro.setInputLimits(-2147483648, 2147483647);
01179 
01180         //制御量の最小、最大
01181         //逆転(目標に達してない)
01182         if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 < target*-1)) {
01183             back_migimae.setOutputLimits(0x00,     0x7B);
01184             back_migiusiro.setOutputLimits(0x00,   0x7B);
01185             //back_hidarimae.setOutputLimits(0x00,   0x73);
01186             //back_hidariusiro.setOutputLimits(0x00, 0x73);
01187             back_hidarimae.setOutputLimits(0x00,   0x7B);
01188             back_hidariusiro.setOutputLimits(0x00, 0x7B);
01189         }
01190         /*
01191         //左側が後に出ちゃった♥(右側だけ回して左側は停止)
01192         else if((y_pulse1*-1 + wheel_difference) < y_pulse2*-1){
01193             back_migimae.setOutputLimits(0x00,     0x7B);
01194             back_migiusiro.setOutputLimits(0x00,   0x7B);
01195             back_hidarimae.setOutputLimits(0x7C,   0x83);
01196             back_hidariusiro.setOutputLimits(0x7C, 0x83);
01197         }
01198         //右側が後に出ちゃった♥(左側だけ回して右側は停止)
01199         else if(y_pulse1*-1 > (y_pulse2*-1 + wheel_difference)){
01200             back_migimae.setOutputLimits(0x7C,     0x83);
01201             back_migiusiro.setOutputLimits(0x7C,   0x83);
01202             back_hidarimae.setOutputLimits(0x00,   0x7B);
01203             back_hidariusiro.setOutputLimits(0x00, 0x7B);
01204         }
01205         */
01206         //停止(目標より行き過ぎ)
01207         else if((y_pulse1*-1 > target*-1) && (y_pulse2*-1 > target*-1)) {
01208             back_migimae.setOutputLimits(0x7C,     0x83);
01209             back_migiusiro.setOutputLimits(0x7C,   0x83);
01210             back_hidarimae.setOutputLimits(0x7C,   0x83);
01211             back_hidariusiro.setOutputLimits(0x7C, 0x83);
01212         }
01213 
01214         //よくわからんやつ
01215         back_migimae.setMode(AUTO_MODE);
01216         back_migiusiro.setMode(AUTO_MODE);
01217         back_hidarimae.setMode(AUTO_MODE);
01218         back_hidariusiro.setMode(AUTO_MODE);
01219 
01220         //目標値
01221         back_migimae.setSetPoint(target*-1);
01222         back_migiusiro.setSetPoint(target*-1);
01223         back_hidarimae.setSetPoint(target*-1);
01224         back_hidariusiro.setSetPoint(target*-1);
01225 
01226         //センサ出力
01227         back_migimae.setProcessValue(y_pulse1*-1);
01228         back_migiusiro.setProcessValue(y_pulse1*-1);
01229         back_hidarimae.setProcessValue(y_pulse2*-1);
01230         back_hidariusiro.setProcessValue(y_pulse2*-1);
01231 
01232         //制御量(計算結果)
01233         migimae_data[0]      = back_migimae.compute();
01234         migiusiro_data[0]    = back_migiusiro.compute();
01235         hidarimae_data[0]    = back_hidarimae.compute();
01236         hidariusiro_data[0]  = back_hidariusiro.compute();
01237 
01238         //制御量をPWM値に変換
01239         //逆転(目標に達してない)
01240         if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 < target*-1)) {
01241             true_migimae_data[0]     = 0x7B - migimae_data[0];
01242             true_migiusiro_data[0]   = 0x7B - migiusiro_data[0];
01243             true_hidarimae_data[0]   = 0x7B - hidarimae_data[0];
01244             true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
01245         }
01246         /*
01247         //左側が後に出ちゃった♥(右側だけ回して左側は停止)
01248         else if((y_pulse1*-1 + wheel_difference) < y_pulse2*-1){
01249             true_migimae_data[0]     = 0x7B - migimae_data[0];
01250             true_migiusiro_data[0]   = 0x7B - migiusiro_data[0];
01251             true_hidarimae_data[0]   = 0x80;
01252             true_hidariusiro_data[0] = 0x80;
01253         }
01254         //右側が後に出ちゃった♥(左側だけ回して右側は停止)
01255         else if(y_pulse1*-1 > (y_pulse2*-1 + wheel_difference)){
01256             true_migimae_data[0]     = 0x80;
01257             true_migiusiro_data[0]   = 0x80;
01258             true_hidarimae_data[0]   = 0x7B - hidarimae_data[0];
01259             true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
01260         }
01261         */
01262         //停止(目標より行き過ぎ)
01263         else if((y_pulse1*-1 > target*-1) && (y_pulse2*-1 > target*-1)) {
01264             true_migimae_data[0]     = 0x80;
01265             true_migiusiro_data[0]   = 0x80;
01266             true_hidarimae_data[0]   = 0x80;
01267             true_hidariusiro_data[0] = 0x80;
01268         }
01269 }
01270 
01271 void right_PID(int target) {
01272 
01273         //センサ出力値の最小、最大
01274         right_migimae.setInputLimits(-2147483648,     2147483647);
01275         right_migiusiro.setInputLimits(-2147483648,   2147483647);
01276         right_hidarimae.setInputLimits(-2147483648,   2147483647);
01277         right_hidariusiro.setInputLimits(-2147483648, 2147483647);
01278 
01279         //制御量の最小、最大
01280         //右進(目標まで達していない)
01281         if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 < target*-1)) {
01282            // right_migimae.setOutputLimits(0x00,     0x6C);
01283             right_migimae.setOutputLimits(0x7A,     0x7B);
01284             right_migiusiro.setOutputLimits(0xFE,   0xFF);
01285             //right_hidarimae.setOutputLimits(0x84,   0xF0);
01286             right_hidarimae.setOutputLimits(0xFE,   0xFF);
01287             right_hidariusiro.setOutputLimits(0x7A, 0x7B);
01288 
01289         }
01290         /*
01291         //前側が右に出ちゃった♥(後側だけ回して前側は停止)
01292         else if(x_pulse1*-1 > (x_pulse2*-1 + wheel_difference)){
01293             right_migimae.setOutputLimits(0x7C,     0x83);
01294             right_migiusiro.setOutputLimits(0x00,   0x7B);
01295             right_hidarimae.setOutputLimits(0x7C,   0x83);
01296             right_hidariusiro.setOutputLimits(0x84, 0xFF);
01297         }
01298         //後側が右に出ちゃった♥(前側だけ回して後側は停止)
01299         else if((x_pulse1*-1 + wheel_difference) < x_pulse2*-1){
01300             right_migimae.setOutputLimits(0x84,     0xFF);
01301             right_migiusiro.setOutputLimits(0x7C,   0x83);
01302             right_hidarimae.setOutputLimits(0x00,   0x7B);
01303             right_hidariusiro.setOutputLimits(0x7C, 0x83);
01304         }
01305         */
01306         //停止(目標より行き過ぎ)
01307         else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 > target*-1)) {
01308             right_migimae.setOutputLimits(0x7C,     0x83);
01309             right_migiusiro.setOutputLimits(0x7C,   0x83);
01310             right_hidarimae.setOutputLimits(0x7C,   0x83);
01311             right_hidariusiro.setOutputLimits(0x7C, 0x83);
01312         }
01313 
01314         //よくわからんやつ
01315         right_migimae.setMode(AUTO_MODE);
01316         right_migiusiro.setMode(AUTO_MODE);
01317         right_hidarimae.setMode(AUTO_MODE);
01318         right_hidariusiro.setMode(AUTO_MODE);
01319 
01320         //目標値
01321         right_migimae.setSetPoint(target*-1);
01322         right_migiusiro.setSetPoint(target*-1);
01323         right_hidarimae.setSetPoint(target*-1);
01324         right_hidariusiro.setSetPoint(target*-1);
01325 
01326         //センサ出力
01327         right_migimae.setProcessValue(x_pulse1*-1);
01328         right_migiusiro.setProcessValue(x_pulse2*-1);
01329         right_hidarimae.setProcessValue(x_pulse1*-1);
01330         right_hidariusiro.setProcessValue(x_pulse2*-1);
01331 
01332         //制御量(計算結果)
01333         migimae_data[0]      = right_migimae.compute();
01334         migiusiro_data[0]    = right_migiusiro.compute();
01335         hidarimae_data[0]    = right_hidarimae.compute();
01336         hidariusiro_data[0]  = right_hidariusiro.compute();
01337 
01338         //制御量をPWM値に変換
01339         //右進(目標まで達していない)
01340         if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 < target*-1)) {
01341             true_migimae_data[0]     = 0x7B - migimae_data[0];
01342             true_migiusiro_data[0]   = migiusiro_data[0];
01343             true_hidarimae_data[0]   = hidarimae_data[0];
01344             true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
01345         }
01346         /*
01347         //前側が右に出ちゃった♥(後側だけ回して前側は停止)
01348         else if(x_pulse1*-1 > (x_pulse2*-1 + wheel_difference)){
01349             true_migimae_data[0]     = 0x80;
01350             true_migiusiro_data[0]   = migiusiro_data[0];
01351             true_hidarimae_data[0]   = 0x80;
01352             true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
01353         }
01354         //後側が右に出ちゃった♥(前側だけ回して後側は停止)
01355         else if((x_pulse1*-1 + wheel_difference) < x_pulse2*-1){
01356             true_migimae_data[0]     = 0x7B - migimae_data[0];
01357             true_migiusiro_data[0]   = 0x80;
01358             true_hidarimae_data[0]   = hidarimae_data[0];
01359             true_hidariusiro_data[0] = 0x80;
01360         }
01361         */
01362         //左進(目標より行き過ぎ)
01363         else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 > target*-1)) {
01364             true_migimae_data[0]     = 0x80;
01365             true_migiusiro_data[0]   = 0x80;
01366             true_hidarimae_data[0]   = 0x80;
01367             true_hidariusiro_data[0] = 0x80;
01368         }
01369 }
01370 
01371 void left_PID(int target) {
01372 
01373         //センサ出力値の最小、最大
01374         left_migimae.setInputLimits(-2147483648,     2147483647);
01375         left_migiusiro.setInputLimits(-2147483648,   2147483647);
01376         left_hidarimae.setInputLimits(-2147483648,   2147483647);
01377         left_hidariusiro.setInputLimits(-2147483648, 2147483647);
01378 
01379         //制御量の最小、最大
01380         //左進(目標まで達していない)
01381         if((x_pulse1 < target) || (x_pulse2 < target)) {
01382             left_migimae.setOutputLimits(0xEC,     0xED);
01383             //left_migimae.setOutputLimits(0x84,     0xFF);
01384             left_migiusiro.setOutputLimits(0x7A,   0x7B);
01385             left_hidarimae.setOutputLimits(0x7A,   0x7B);
01386             left_hidariusiro.setOutputLimits(0xFE, 0xFF);
01387         }
01388         /*
01389         //前側が左に出ちゃった♥(後側だけ回して前側は停止)
01390         else if(x_pulse1 > (x_pulse2 + wheel_difference)){
01391             left_migimae.setOutputLimits(0x7C,     0x83);
01392             left_migiusiro.setOutputLimits(0x7C,   0x83);
01393             left_hidarimae.setOutputLimits(0x10,   0x7B);
01394             left_hidariusiro.setOutputLimits(0x94, 0xFF);
01395         }
01396         //後側が左に出ちゃった♥(前側だけ回して後側は停止)
01397         else if((x_pulse1 + wheel_difference) < x_pulse2){
01398             left_migimae.setOutputLimits(0x94,     0xFF);
01399             left_migiusiro.setOutputLimits(0x10,   0x7B);
01400             left_hidarimae.setOutputLimits(0x7C,   0x83);
01401             left_hidariusiro.setOutputLimits(0x7C, 0x83);
01402         }
01403         */
01404         //停止(目標より行き過ぎ)
01405         else if((x_pulse1 > target) || (x_pulse2 > target)) {
01406             left_migimae.setOutputLimits(0x7C,     0x83);
01407             left_migiusiro.setOutputLimits(0x7C,   0x83);
01408             left_hidarimae.setOutputLimits(0x7C,   0x83);
01409             left_hidariusiro.setOutputLimits(0x7C, 0x83);
01410         }
01411 
01412         //よくわからんやつ
01413         left_migimae.setMode(AUTO_MODE);
01414         left_migiusiro.setMode(AUTO_MODE);
01415         left_hidarimae.setMode(AUTO_MODE);
01416         left_hidariusiro.setMode(AUTO_MODE);
01417 
01418         //目標値
01419         left_migimae.setSetPoint(target);
01420         left_migiusiro.setSetPoint(target);
01421         left_hidarimae.setSetPoint(target);
01422         left_hidariusiro.setSetPoint(target);
01423 
01424         //センサ出力
01425         left_migimae.setProcessValue(x_pulse1);
01426         left_migiusiro.setProcessValue(x_pulse2);
01427         left_hidarimae.setProcessValue(x_pulse1);
01428         left_hidariusiro.setProcessValue(x_pulse2);
01429 
01430         //制御量(計算結果)
01431         migimae_data[0]      = left_migimae.compute();
01432         migiusiro_data[0]    = left_migiusiro.compute();
01433         hidarimae_data[0]    = left_hidarimae.compute();
01434         hidariusiro_data[0]  = left_hidariusiro.compute();
01435 
01436         //制御量をPWM値に変換
01437         //左進(目標まで達していない)
01438         if((x_pulse1 < target) || (x_pulse2 < target)) {
01439             true_migimae_data[0]     = migimae_data[0];
01440             true_migiusiro_data[0]   = 0x7B - migiusiro_data[0];
01441             true_hidarimae_data[0]   = 0x7B - hidarimae_data[0];
01442             true_hidariusiro_data[0] = hidariusiro_data[0];
01443         }
01444         /*
01445         //前側が左に出ちゃった♥(後側だけ回して前側は停止)
01446         else if(x_pulse1 > (x_pulse2 + wheel_difference)){
01447             true_migimae_data[0]     = 0x80;
01448             true_migiusiro_data[0]   = 0x80;
01449             true_hidarimae_data[0]   = 0x7B - hidarimae_data[0];
01450             true_hidariusiro_data[0] = hidariusiro_data[0];
01451         }
01452         //後側が左に出ちゃった♥(前側だけ回して後側は停止)
01453         else if((x_pulse1 + wheel_difference) < x_pulse2){
01454             true_migimae_data[0]     = migimae_data[0];
01455             true_migiusiro_data[0]   = 0x7B - migiusiro_data[0];
01456             true_hidarimae_data[0]   = 0x80;
01457             true_hidariusiro_data[0] = 0x80;
01458         }
01459         */
01460         //停止(目標より行き過ぎ)
01461         else if((x_pulse1 > target) || (x_pulse2 > target)) {
01462             true_migimae_data[0]     = 0x80;
01463             true_migiusiro_data[0]   = 0x80;
01464             true_hidarimae_data[0]   = 0x80;
01465             true_hidariusiro_data[0] = 0x80;
01466         }
01467 }
01468 
01469 void turn_right_PID(int target) {
01470 
01471         //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
01472         turn_right_migimae.setInputLimits(-2147483648,     2147483647);
01473         turn_right_migiusiro.setInputLimits(-2147483648,   2147483647);
01474         turn_right_hidarimae.setInputLimits(-2147483648,   2147483647);
01475         turn_right_hidariusiro.setInputLimits(-2147483648, 2147483647);
01476 
01477         //制御量の最小、最大
01478         //右旋回(目標に達してない)
01479         if(x_pulse2 < target) {
01480             turn_right_migimae.setOutputLimits(0x10,   0x7B);
01481             turn_right_migiusiro.setOutputLimits(0x10, 0x7B);
01482             turn_right_hidarimae.setOutputLimits(0x94, 0xFF);
01483             turn_right_hidariusiro.setOutputLimits(0x94, 0xFF);
01484         }
01485         //停止(目標より行き過ぎ)
01486         else if(x_pulse2 > target) {
01487             turn_right_migimae.setOutputLimits(0x7C,     0x83);
01488             turn_right_migiusiro.setOutputLimits(0x7C,   0x83);
01489             turn_right_hidarimae.setOutputLimits(0x7C,   0x83);
01490             turn_right_hidariusiro.setOutputLimits(0x7C, 0x83);
01491         }
01492 
01493         //よくわからんやつ
01494         turn_right_migimae.setMode(AUTO_MODE);
01495         turn_right_migiusiro.setMode(AUTO_MODE);
01496         turn_right_hidarimae.setMode(AUTO_MODE);
01497         turn_right_hidariusiro.setMode(AUTO_MODE);
01498 
01499         //目標値
01500         turn_right_migimae.setSetPoint(target);
01501         turn_right_migiusiro.setSetPoint(target);
01502         turn_right_hidarimae.setSetPoint(target);
01503         turn_right_hidariusiro.setSetPoint(target);
01504 
01505         //センサ出力
01506         turn_right_migimae.setProcessValue(x_pulse2);
01507         turn_right_migiusiro.setProcessValue(x_pulse2);
01508         turn_right_hidarimae.setProcessValue(x_pulse2);
01509         turn_right_hidariusiro.setProcessValue(x_pulse2);
01510 
01511         //制御量(計算結果)
01512         migimae_data[0]      = turn_right_migimae.compute();
01513         migiusiro_data[0]    = turn_right_migiusiro.compute();
01514         hidarimae_data[0]    = turn_right_hidarimae.compute();
01515         hidariusiro_data[0]  = turn_right_hidariusiro.compute();
01516 
01517         //制御量をPWM値に変換
01518         //右旋回(目標に達してない)
01519         if(x_pulse2 < target) {
01520             true_migimae_data[0]     = 0x7B - migimae_data[0];
01521             true_migiusiro_data[0]   = 0x7B - migiusiro_data[0];
01522             true_hidarimae_data[0]   = hidarimae_data[0];
01523             true_hidariusiro_data[0] = hidariusiro_data[0];
01524         }
01525         //停止(目標より行き過ぎ)
01526         else if(x_pulse2 > target) {
01527             true_migimae_data[0]     = 0x80;
01528             true_migiusiro_data[0]   = 0x80;
01529             true_hidarimae_data[0]   = 0x80;
01530             true_hidariusiro_data[0] = 0x80;
01531         }
01532 }
01533 
01534 void turn_left_PID(int target) {
01535 
01536         //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
01537         turn_left_migimae.setInputLimits(-2147483648,     2147483647);
01538         turn_left_migiusiro.setInputLimits(-2147483648,   2147483647);
01539         turn_left_hidarimae.setInputLimits(-2147483648,   2147483647);
01540         turn_left_hidariusiro.setInputLimits(-2147483648, 2147483647);
01541 
01542         //制御量の最小、最大
01543         //左旋回(目標に達してない)
01544         if(x_pulse1 < target) {
01545             turn_left_migimae.setOutputLimits(0x94,   0xFF);
01546             turn_left_migiusiro.setOutputLimits(0x94, 0xFF);
01547             turn_left_hidarimae.setOutputLimits(0x10, 0x7B);
01548             turn_left_hidariusiro.setOutputLimits(0x10, 0x7B);
01549         }
01550         //停止(目標より行き過ぎ)
01551         else if(x_pulse1 > target) {
01552             turn_left_migimae.setOutputLimits(0x7C,     0x83);
01553             turn_left_migiusiro.setOutputLimits(0x7C,   0x83);
01554             turn_left_hidarimae.setOutputLimits(0x7C,   0x83);
01555             turn_left_hidariusiro.setOutputLimits(0x7C, 0x83);
01556         }
01557 
01558         //よくわからんやつ
01559         turn_left_migimae.setMode(AUTO_MODE);
01560         turn_left_migiusiro.setMode(AUTO_MODE);
01561         turn_left_hidarimae.setMode(AUTO_MODE);
01562         turn_left_hidariusiro.setMode(AUTO_MODE);
01563 
01564         //目標値
01565         turn_left_migimae.setSetPoint(target);
01566         turn_left_migiusiro.setSetPoint(target);
01567         turn_left_hidarimae.setSetPoint(target);
01568         turn_left_hidariusiro.setSetPoint(target);
01569 
01570         //センサ出力
01571         turn_left_migimae.setProcessValue(x_pulse1);
01572         turn_left_migiusiro.setProcessValue(x_pulse1);
01573         turn_left_hidarimae.setProcessValue(x_pulse1);
01574         turn_left_hidariusiro.setProcessValue(x_pulse1);
01575 
01576         //制御量(計算結果)
01577         migimae_data[0]      = turn_left_migimae.compute();
01578         migiusiro_data[0]    = turn_left_migiusiro.compute();
01579         hidarimae_data[0]    = turn_left_hidarimae.compute();
01580         hidariusiro_data[0]  = turn_left_hidariusiro.compute();
01581 
01582         //制御量をPWM値に変換
01583         //左旋回(目標に達してない)
01584         if(x_pulse1 < target) {
01585             true_migimae_data[0]     = migimae_data[0];
01586             true_migiusiro_data[0]   = migiusiro_data[0];
01587             true_hidarimae_data[0]   = 0x7B - hidarimae_data[0];
01588             true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
01589         }
01590         //左旋回(目標より行き過ぎ)
01591         else if(x_pulse1 > target) {
01592             true_migimae_data[0]     = 0x80;
01593             true_migiusiro_data[0]   = 0x80;
01594             true_hidarimae_data[0]   = 0x80;
01595             true_hidariusiro_data[0] = 0x80;
01596         }
01597 }