圭祐 杉浦
本番用
Dependencies: MotorDriver_SU USBDevice mbed
Diff: main.cpp
- Revision:
- 1:f5a011cef88b
- Parent:
- 0:fa4301e041d0
- Child:
- 2:ac16e274c6d5
--- a/main.cpp Sat Nov 08 10:57:34 2014 +0000
+++ b/main.cpp Sat Nov 08 15:31:47 2014 +0000
@@ -20,7 +20,7 @@
float hoge3;
float hoge4;
float hoge5;
-
+ int AI = 0;
/********************************/
@@ -39,397 +39,63 @@
hoge4 = sw4.read();
hoge5 = sw5.read();
- while( sw == 0 ){
- motor.Drive(0, STOP, 0);
- motor.Drive(1, STOP, 0);
- }
+
/************************************/
/*ラインに沿って走るためのプログラム*/
/************************************/
-
- /*1 センサ１が黒*/
- if(sw1 <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
- motor.Drive(0, CW, 1500);
+ ///////////まっすぐ走るためのプログラム//////////
+ /*1.1.1 センサ１が黒*/
+ if(sw1 <= 0.3 && sw2 > 0.3 && sw3 > 0.3 && sw4 > 0.3 && sw5 > 0.3){
+ motor.Drive(0, CW, 1000);
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);
+ /*1.1.2 センサ2が黒*/
+ else if(sw1 > 0.3 && sw2 <= 0.3 && sw3 > 0.3 && sw4 > 0.3 && sw5 > 0.3){
+ motor.Drive(0, CW, 1250);
motor.Drive(1, CW, 4095);
}
- /*3 センサ3が黒*/
- else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){
+ /*1.1.3 センサ3が黒*/
+ else if(sw1 > 0.3 && sw2 > 0.3 && sw3 <= 0.3 && sw4 > 0.3 && sw5 > 0.3){
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){
+ /*1.1.4 センサ4が黒*/
+ else if(sw1 > 0.3 && sw2 > 0.3 && sw3 > 0.3 && sw4 <= 0.3 && sw5 > 0.3){
motor.Drive(0, CW, 4095);
- motor.Drive(1, CW, 2000);
+ motor.Drive(1, CW, 1250);
}
- /*5 センサ5が黒*/
- else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 <= 0.3){
+ /*1.1.5 センサ5が黒*/
+ else if(sw1 > 0.3 && sw2 > 0.3 && sw3 > 0.3 && sw4 > 0.3 && sw5 <= 0.3){
motor.Drive(0, CW, 4095);
- motor.Drive(1, CW, 1500);
+ motor.Drive(1, CW, 1000);
}
- /*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);
+ /*1.1.6 センサ2-3が黒*/
+ else if(sw1 > 0.3 && sw2 <= 0.3 && sw3 <= 0.3 && sw4 > 0.3 && sw5 > 0.3){
+ motor.Drive(0, CW, 1500);
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){
+ /*1.1.7 センサ3-4が黒*/
+ else if(sw1 > 0.3 && sw2 > 0.3 && sw3 <= 0.3 && sw4 <= 0.3 && sw5 > 0.3){
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 センサ１が黒*/
- 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);
+ motor.Drive(1, CW, 1500);
}
-
-
- /*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);
-
-
-
-}
\ No newline at end of file
+}
+}
\ No newline at end of file