本番用
Dependencies: MotorDriver_SU USBDevice mbed
main.cpp
- Committer:
- 12f2027a
- Date:
- 2014-11-08
- Revision:
- 0:fa4301e041d0
- Child:
- 1:f5a011cef88b
File content as of revision 0:fa4301e041d0:
#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_14); 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; /********************************/ /********************************/ /********************************/ /*荷物を持ち上げる前のプログラム*/ /********************************/ /********************************/ /********************************/ 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); } /************************************/ /*ラインに沿って走るためのプログラム*/ /************************************/ /*1 センサ1が黒*/ if(sw1 <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){ motor.Drive(0, CW, 1500); motor.Drive(1, CW, 4095); } /*2 センサ2が黒*/ else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){ motor.Drive(0, CW, 2000); motor.Drive(1, CW, 4095); } /*3 センサ3が黒*/ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){ motor.Drive(0, CW, 4095); motor.Drive(1, CW, 4095); } /*4 センサ4が黒*/ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 > 0.5){ motor.Drive(0, CW, 4095); motor.Drive(1, CW, 2000); } /*5 センサ5が黒*/ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 <= 0.3){ motor.Drive(0, CW, 4095); motor.Drive(1, CW, 1500); } /*6 センサ2-3が黒*/ else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){ motor.Drive(0, CW, 3000); motor.Drive(1, CW, 4095); } /*7 センサ2-5が黒*/ else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 &&sw5 <= 0.5){ motor.Drive(0, CW, 4095); motor.Drive(1, STOP, 0); } /*8 センサ3-4が黒*/ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 <= 0.3 && sw5 > 0.5){ motor.Drive(0, CW, 4095); motor.Drive(1, CW, 3000); } /*9 センサ3-5が黒*/ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 <= 0.5){ motor.Drive(0, CW, 4095); motor.Drive(1, STOP, 0); } /*10 センサ4-5が黒*/ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 <= 0.5){ motor.Drive(0, CW, 4095); motor.Drive(1, STOP, 0); } /*11 センサ2-3-5が黒*/ else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 <= 0.3 && sw4 > 0.5 && sw5 <= 0.5){ motor.Drive(0, CW, 4095); motor.Drive(1, STOP, 0); } /*12 センサ3-4-5が黒*/ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 <= 0.3 && sw5 <= 0.5){ motor.Drive(0, CW, 4095); motor.Drive(1, STOP, 0); } /*13 全部白*/ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){ motor.Drive(0, CW, 4095); motor.Drive(1, CCW, 4095); } /**********************************/ /*停止位置で止まるためのプログラム*/ /**********************************/ /*14 センサ1-3-5が黒*/ else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.5){ motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); break; } /*15 センサ1-2-5が黒*/ else if(sw1 <= 0.5 && sw2 <= 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 <= 0.5){ motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); break; } /*16 センサ1-4-5が黒*/ else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.5 && sw5 <= 0.5){ motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); break; } /*17 センサ1-2-3-5が黒*/ else if(sw1 <= 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.5){ motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); break; } /*18 センサ1-2-3-5が黒*/ else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 <= 0.5){ motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); break; } /*19 センサ1-4-5が黒*/ else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.5 && sw5 <= 0.5){ motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); break; } /*19 センサ1-2-5が黒*/ else if(sw1 <= 0.5 && sw2 <= 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 <= 0.5){ motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); break; } /*21 センサ1-3-4が黒*/ else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){ motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); break; } /*22 センサ1-4が黒*/ else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.5 && sw5 > 0.5){ motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); break; } } motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); wait(0.1); lift.Drive(0, CCW, 4095); wait(1.5); lift.Drive(0, STOP, 0); wait(0.1); motor.Drive(0, CW, 4095); motor.Drive(1, CW, 4095); wait(1.0); /********************************/ /********************************/ /********************************/ /*荷物を持ち上げた後のプログラム*/ /********************************/ /********************************/ /********************************/ while(1){ hoge1 = sw1.read(); hoge2 = sw2.read(); hoge3 = sw3.read(); hoge4 = sw4.read(); hoge5 = sw5.read(); /************************************/ /*ラインに沿って走るためのプログラム*/ /************************************/ /*1 センサ1が黒*/ if(sw1 <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){ motor.Drive(0, CW, 1500); motor.Drive(1, CW, 4095); } /*2 センサ2が黒*/ else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){ motor.Drive(0, CW, 2000); motor.Drive(1, CW, 4095); } /*3 センサ3が黒*/ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){ motor.Drive(0, CW, 4095); motor.Drive(1, CW, 4095); } /*4 センサ4が黒*/ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 > 0.5){ motor.Drive(0, CW, 4095); motor.Drive(1, CW, 2000); } /*5 センサ5が黒*/ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 <= 0.3){ motor.Drive(0, CW, 4095); motor.Drive(1, CW, 1500); } /*6 センサ2-3が黒*/ else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){ motor.Drive(0, CW, 3000); motor.Drive(1, CW, 4095); } /*7 センサ2-5が黒*/ else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 &&sw5 <= 0.5){ motor.Drive(0, CW, 4095); motor.Drive(1, STOP, 0); } /*8 センサ3-4が黒*/ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 <= 0.3 && sw5 > 0.5){ motor.Drive(0, CW, 4095); motor.Drive(1, CW, 3000); } /*9 センサ3-5が黒*/ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 <= 0.5){ motor.Drive(0, CW, 4095); motor.Drive(1, STOP, 0); } /*10 センサ4-5が黒*/ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 <= 0.5){ motor.Drive(0, CW, 4095); motor.Drive(1, STOP, 0); } /*11 センサ2-3-5が黒*/ else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 <= 0.3 && sw4 > 0.5 && sw5 <= 0.5){ motor.Drive(0, CW, 4095); motor.Drive(1, STOP, 0); } /*12 センサ3-4-5が黒*/ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 <= 0.3 && sw5 <= 0.5){ motor.Drive(0, CW, 4095); motor.Drive(1, STOP, 0); } /*13 全部白*/ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){ motor.Drive(0, CW, 4095); motor.Drive(1, CCW, 4095); } /**********************************/ /*停止位置で止まるためのプログラム*/ /**********************************/ /*14 センサ1-3-5が黒*/ else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.5){ motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); break; } /*15 センサ1-2-5が黒*/ else if(sw1 <= 0.5 && sw2 <= 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 <= 0.5){ motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); break; } /*16 センサ1-4-5が黒*/ else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.5 && sw5 <= 0.5){ motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); break; } /*17 センサ1-2-3-5が黒*/ else if(sw1 <= 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.5){ motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); break; } /*18 センサ1-2-3-5が黒*/ else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 <= 0.5){ motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); break; } /*19 センサ1-4-5が黒*/ else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.5 && sw5 <= 0.5){ motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); break; } /*19 センサ1-2-5が黒*/ else if(sw1 <= 0.5 && sw2 <= 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 <= 0.5){ motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); break; } /*21 センサ1-3-4が黒*/ else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){ motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); break; } /*22 センサ1-4が黒*/ else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.5 && sw5 > 0.5){ motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); break; } } 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(2.0); motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); wait(1.0); }