候補
Dependencies: MotorDriver_SU mbed
center2.cpp
- Committer:
- 12f2027a
- Date:
- 2014-12-01
- Revision:
- 0:41b88efe25c9
File content as of revision 0:41b88efe25c9:
#include "mbed.h" #include "MotorDriver_SU.h" DigitalIn sw(P0_1, PullUp); DigitalOut led(P1_28); AnalogIn sw1(P0_22); AnalogIn sw2(P0_12); AnalogIn sw3(P0_16); AnalogIn sw4(P0_11); AnalogIn sw5(P0_13); MotorDriver_SU motor(MOTOR_DOUBLE); MotorDriver_SU lift(MOTOR_SINGLE); int main( ){ float hoge1; float hoge2; float hoge3; float hoge4; float hoge5; int position = 0; int AI = 0;//カーブの方向判定 int updown = 0;//リフトの向き判定 int R = 0; int L = 0; //11111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 while(1){ hoge1 = sw1.read(); hoge2 = sw2.read(); hoge3 = sw3.read(); hoge4 = sw4.read(); hoge5 = sw5.read(); while(sw == 0){ motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); } /*黒_白_白_白_白*/ if(sw1 <= 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 > 0.6){ motor.Drive(0, CW, 500); motor.Drive(1, CW, 2000); } /*白_黒_白_白_白*/ else if(sw1 > 0.6 && sw2 <= 0.3 && sw3 > 0.6 && sw4 > 0.6 && sw5 > 0.6){ motor.Drive(0, CW, 1600); motor.Drive(1, CW, 2000); } /*白_黒_黒_白_白*/ else if(sw1 > 0.6 && sw2 <= 0.6 && sw3 <= 0.6 && sw4 > 0.6 && sw5 > 0.6){ motor.Drive(0, CW, 1800); motor.Drive(1, CW, 2000); } /*白_白_黒_白_白*/ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.3 && sw4 > 0.6 && sw5 > 0.6){ motor.Drive(0, CW, 2000); motor.Drive(1, CW, 2000); } /*白_白_黒_黒_白*/ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.6 && sw4 <= 0.6 && sw5 > 0.6){ motor.Drive(0, CW, 2000); motor.Drive(1, CW, 1800); } /*白_白_白_黒_白*/ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 <= 0.3 && sw5 > 0.6){ motor.Drive(0, CW, 2000); motor.Drive(1, CW, 1600); } /*白_白_白_白_黒*/ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 <= 0.6){ motor.Drive(0, CW, 2000); motor.Drive(1, CW, 500); } else if(sw1 > 0.5 && sw5 <= 0.3){ break; } } //2222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222 while(1){ hoge1 = sw1.read(); hoge2 = sw2.read(); hoge3 = sw3.read(); hoge4 = sw4.read(); hoge5 = sw5.read(); while(sw == 0){ motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); } /*黒_白_白_白_白*/ if(sw1 <= 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 > 0.6){ motor.Drive(0, CW, 500); motor.Drive(1, CW, 1000); } /*黒_白_白*/ else if(sw2 <= 0.3 && sw3 > 0.6 && sw4 > 0.6){ motor.Drive(0, CW, 600); motor.Drive(1, CW, 1000); } /*黒_黒_白*/ else if(sw1 > 0.6 && sw2 <= 0.6 && sw3 <= 0.6 && sw4 > 0.6 && sw5 > 0.6){ motor.Drive(0, CW, 1000); motor.Drive(1, CW, 200); } /*白_黒_白*/ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.3 && sw4 > 0.6 && sw5 > 0.6){ motor.Drive(0, CW, 1000); motor.Drive(1, CW, 1000); } /*白_黒_黒*/ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.6 && sw4 <= 0.6 && sw5 > 0.6){ motor.Drive(0, CW, 1000); motor.Drive(1, CW, 200); } /*白_白_黒*/ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 <= 0.3 && sw5 > 0.6){ motor.Drive(0, CW, 1000); motor.Drive(1, CW, 600); } /*白_白_白_白_黒*/ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 <= 0.6){ motor.Drive(0, CW, 1000); motor.Drive(1, CW, 500); } else if(sw1 <= 0.6 ){ motor.Drive(0, CW, 1000); motor.Drive(1, CCW, 1000); wait(0.225); break; } } //3333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333 while(1){ hoge1 = sw1.read(); hoge2 = sw2.read(); hoge3 = sw3.read(); hoge4 = sw4.read(); hoge5 = sw5.read(); while(sw == 0){ motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); } /*黒_白_白_白_白*/ if(sw1 <= 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 > 0.6){ motor.Drive(0, CW, 300); motor.Drive(1, CW, 1000); } /*白_白_白*/ else if(sw2 <= 0.6 && sw3 > 0.6 && sw4 > 0.6){ motor.Drive(0, CW, 400); motor.Drive(1, CW, 1000); } /*黒_白_白*/ else if(sw2 <= 0.3 && sw3 > 0.6 && sw4 > 0.6){ motor.Drive(0, CW, 1000); motor.Drive(1, CW, 600); } /*黒_黒_白*/ else if(sw1 > 0.6 && sw2 <= 0.6 && sw3 <= 0.6 && sw4 > 0.6 && sw5 > 0.6){ motor.Drive(0, CW, 1000); motor.Drive(1, CW, 500); } /*白_黒_白*/ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.3 && sw4 > 0.6 && sw5 > 0.6){ motor.Drive(0, CW, 1000); motor.Drive(1, CW, 400); } /*白_黒_黒*/ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.6 && sw4 <= 0.6 && sw5 > 0.6){ motor.Drive(0, CW, 1000); motor.Drive(1, CW, 300); } /*白_白_黒*/ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 <= 0.3 && sw5 > 0.6){ motor.Drive(0, CW, 1000); motor.Drive(1, CW, 300); } /*白_白_白_白_黒*/ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 <= 0.6){ motor.Drive(0, CW, 1000); motor.Drive(1, CW, 300); } else if(sw1 > 0.6 && sw5 <= 0.6){ motor.Drive(0, CW, 1000); motor.Drive(1, CCW, 1000); } else if(sw1 <= 0.6 && sw5 > 0.6){ motor.Drive(0, CW, 1000); motor.Drive(1, CW, 500); wait(0.2); break; } } //444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444 while(1){ hoge1 = sw1.read(); hoge2 = sw2.read(); hoge3 = sw3.read(); hoge4 = sw4.read(); hoge5 = sw5.read(); while(sw == 0){ motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); } /*黒_白_白_白_白*/ if(sw1 <= 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 > 0.6){ motor.Drive(0, CW, 500); motor.Drive(1, CW, 1000); } /*黒_白_白*/ else if(sw1 > 0.6 && sw2 <= 0.3 && sw3 > 0.6 && sw4 > 0.6 && sw5 > 0.6){ motor.Drive(0, CW, 400); motor.Drive(1, CW, 1000); } /*黒_黒_白*/ else if(sw1 > 0.6 && sw2 <= 0.6 && sw3 <= 0.6 && sw4 > 0.6 && sw5 > 0.6){ motor.Drive(0, CW, 350); motor.Drive(1, CW, 1000); } /*白_黒_白*/ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.3 && sw4 > 0.6 && sw5 > 0.6){ motor.Drive(0, CW, 1000); motor.Drive(1, CW, 1000); } /*白_黒_黒*/ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.6 && sw4 <= 0.6 && sw5 > 0.6){ motor.Drive(0, CW, 350); motor.Drive(1, CW, 1000); } /*白_白_黒*/ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 <= 0.3 && sw5 > 0.6){ motor.Drive(0, CW, 1000); motor.Drive(1, CW, 400); } /*白_白_白_白_黒*/ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 <= 0.6){ motor.Drive(0, CW, 1000); motor.Drive(1, CW, 500); } else if(sw5 <= 0.3){ motor.Drive(0, CW, 1000); motor.Drive(1, CW, 1000); wait(0.28); break; } } //5555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555 while(1){ hoge1 = sw1.read(); hoge2 = sw2.read(); hoge3 = sw3.read(); hoge4 = sw4.read(); hoge5 = sw5.read(); while(sw == 0){ motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); } /*黒_白_白_白_白*/ if(sw1 <= 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 > 0.6){ motor.Drive(0, CW, 800); motor.Drive(1, CW, 1200); } /*白_黒_白_白_白*/ else if(sw1 > 0.6 && sw2 <= 0.3 && sw3 > 0.6 && sw4 > 0.6 && sw5 > 0.6){ motor.Drive(0, CW, 800); motor.Drive(1, CW, 1200); } /*白_黒_黒_白_白*/ else if(sw1 > 0.6 && sw2 <= 0.6 && sw3 <= 0.6 && sw4 > 0.6 && sw5 > 0.6){ motor.Drive(0, CW, 1000); motor.Drive(1, CW, 1200); } /*白_白_黒_白_白*/ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.3 && sw4 > 0.6 && sw5 > 0.6){ motor.Drive(0, CW, 1200); motor.Drive(1, CW, 1200); } /*白_白_黒_黒_白*/ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.6 && sw4 <= 0.6 && sw5 > 0.6){ motor.Drive(0, CW, 1200); motor.Drive(1, CW, 1000); } /*白_白_白_黒_白*/ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 <= 0.3 && sw5 > 0.6){ motor.Drive(0, CW, 1200); motor.Drive(1, CW, 1000); } /*白_白_白_白_黒*/ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 <= 0.6){ motor.Drive(0, CW, 1200); motor.Drive(1, CW, 800); } ///////////大きい右カーブを曲がるためのプログラム////////// /*1センサ2-5が黒(センサ5によって右のカーブであることを判定する)*/ else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 &&sw5 <= 0.3){ AI = 1; } /*2センサ3-5が黒(センサ5によって右のカーブであることを判定する)*/ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 <= 0.3){ AI = 1; } /*3センサ4-5が黒(センサ5によって右のカーブであることを判定する)*/ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 <= 0.3){ AI = 1; } /*4センサ2-3-5が黒(センサ5によって右のカーブであることを判定する)*/ else if(sw1 > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.3){ AI = 1; } /*5センサ3-4-5が黒(センサ5によって右のカーブであることを判定する)*/ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 <= 0.3 && sw5 <= 0.3){ AI = 1; } ///////////大きい左カーブを曲がるためのプログラム////////// /*6センサ1-2が黒(センサ1によって右のカーブであることを判定する)*/ else if(sw1 <= 0.3 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){ AI = 2; } /*7センサ1-3が黒(センサ1によって右のカーブであることを判定する)*/ else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){ AI = 2; } /*8センサ1-4が黒(センサ1によって右のカーブであることを判定する)*/ else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 > 0.5){ AI = 2; } /*9センサ1-2-3が黒(センサ1によって右のカーブであることを判定する)*/ else if(sw1 <= 0.3 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){ AI = 2; } /*10センサ1-3が黒(センサ1によって右のカーブであることを判定する)*/ else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){ AI = 2; } /*11全部白かつAI = 1*/ else if(AI == 1 && sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){ motor.Drive(0, CW, 1200); motor.Drive(1, CCW, 1200); } /*12全部白かつAI = 2*/ else if(AI == 2 && sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){ motor.Drive(0, CCW, 1200); motor.Drive(1, CW, 1200); } /**********************************/ /*停止位置で止まるためのプログラム*/ /**********************************/ /*1.4.1 センサ1-5が黒*/ else if(sw1 <= 0.5 && sw5 <= 0.5){ if( updown == 0 ){ ///////////停止////////// /* motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); wait(0.1); */ ///////////リフト上げる////////// lift.Drive(0, CCW, 2000); wait(1.5); ///////////リフト停止////////// lift.Drive(0, STOP, 0); wait(0.1); ///////////停止位置を超えるために少し前進////////// /* motor.Drive(0, CW, 1200); motor.Drive(1, CW, 1200); wait(0.1); */ updown = 1; } else if( updown == 1 ){ ///////////停止////////// motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); wait(0.1); ///////////リフト下げる////////// lift.Drive(0, CW, 4095); wait(1.0); ///////////リフト停止////////// lift.Drive(0, STOP, 0); wait(0.1); ///////////バック////////// motor.Drive(0, CCW, 4095); motor.Drive(1, CCW, 4095); wait(1.2); ///////////モータ停止////////// motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); wait(1.0); updown = 0; break; } } } }