圭祐 杉浦
本番用
Dependencies: MotorDriver_SU USBDevice mbed
Diff: main.cpp
- Revision:
- 2:ac16e274c6d5
- Parent:
- 1:f5a011cef88b
--- a/main.cpp Sat Nov 08 15:31:47 2014 +0000
+++ b/main.cpp Sat Nov 08 18:55:39 2014 +0000
@@ -39,30 +39,33 @@
hoge4 = sw4.read();
hoge5 = sw5.read();
-
+ while( sw == 0 ){
+ motor.Drive(0, CW, 0);
+ motor.Drive(1, CW, 0);
+
+ }
/************************************/
/*ラインに沿って走るためのプログラム*/
/************************************/
-
///////////まっすぐ走るためのプログラム//////////
/*1.1.1 センサ１が黒*/
- if(sw1 <= 0.3 && sw2 > 0.3 && sw3 > 0.3 && sw4 > 0.3 && sw5 > 0.3){
- motor.Drive(0, CW, 1000);
+ if(sw1 <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
+ motor.Drive(0, CW, 250);
motor.Drive(1, CW, 4095);
}
/*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);
+ else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
+ motor.Drive(0, CW, 500);
motor.Drive(1, CW, 4095);
}
/*1.1.3 センサ3が黒*/
- else if(sw1 > 0.3 && sw2 > 0.3 && sw3 <= 0.3 && sw4 > 0.3 && sw5 > 0.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);
@@ -70,32 +73,370 @@
/*1.1.4 センサ4が黒*/
- else if(sw1 > 0.3 && sw2 > 0.3 && sw3 > 0.3 && sw4 <= 0.3 && sw5 > 0.3){
+ 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, CW, 1250);
+ motor.Drive(1, CW, 500);
}
/*1.1.5 センサ5が黒*/
- else if(sw1 > 0.3 && sw2 > 0.3 && sw3 > 0.3 && sw4 > 0.3 && sw5 <= 0.3){
+ 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, 1000);
+ motor.Drive(1, CW, 250);
}
/*1.1.6 センサ2-3が黒*/
- else if(sw1 > 0.3 && sw2 <= 0.3 && sw3 <= 0.3 && sw4 > 0.3 && sw5 > 0.3){
+ else if(sw1 > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){
motor.Drive(0, CW, 1500);
motor.Drive(1, CW, 4095);
}
/*1.1.7 センサ3-4が黒*/
- else if(sw1 > 0.3 && sw2 > 0.3 && sw3 <= 0.3 && sw4 <= 0.3 && sw5 > 0.3){
+ 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, CW, 1500);
}
-}
-}
\ No newline at end of file
+
+ ///////////大きい右カーブを曲がるためのプログラム//////////
+ /*1.2.1 センサ2-5が黒（センサ5によって右のカーブであることを判定する）*/
+ else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 &&sw5 <= 0.3){
+ AI = 1;
+ }
+
+ /*1.2.2 センサ3-5が黒（センサ5によって右のカーブであることを判定する）*/
+ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 <= 0.3){
+ AI = 1;
+ }
+ /*1.2.3 センサ4-5が黒（センサ5によって右のカーブであることを判定する）*/
+ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 <= 0.3){
+ AI = 1;
+ }
+
+ /*1.2..3 センサ2-3-5が黒（センサ5によって右のカーブであることを判定する）*/
+ else if(sw1 > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.3){
+ AI = 1;
+ }
+
+ /*1.2.4 センサ3-4-5が黒（センサ5によって右のカーブであることを判定する）*/
+ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 <= 0.3 && sw5 <= 0.3){
+ AI = 1;
+ }
+
+ /*1.2.5 全部白かつ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, 4095);
+ motor.Drive(1, CCW, 4095);
+
+ }
+
+ ///////////大きい左カーブを曲がるためのプログラム//////////
+ /*1.3.1 センサ1-2が黒（センサ1によって右のカーブであることを判定する）*/
+ else if(sw1 <= 0.3 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
+ AI = 2;
+ }
+
+ /*1.3.2 センサ1-3が黒（センサ1によって右のカーブであることを判定する）*/
+ else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){
+ AI = 2;
+ }
+
+ /*1.3.3 センサ1-4が黒（センサ1によって右のカーブであることを判定する）*/
+ else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 > 0.5){
+ AI = 2;
+ }
+
+ /*1.3.4 センサ1-2-3が黒（センサ1によって右のカーブであることを判定する）*/
+ else if(sw1 <= 0.3 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){
+ AI = 2;
+ }
+
+ /*1.3.5 センサ1-3が黒（センサ1によって右のカーブであることを判定する）*/
+ else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){
+ AI = 2;
+ }
+
+ /*1.3.6 全部白かつ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, 4095);
+ motor.Drive(1, CW, 4095);
+
+ }
+
+ /**********************************/
+ /*停止位置で止まるためのプログラム*/
+ /**********************************/
+
+ /*1.4.1 センサ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);
+ AI = 0;
+ break;
+
+ }
+
+ /*1.4.2 センサ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);
+ AI = 0;
+ break;
+
+ }
+
+ /*1.4.3 センサ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);
+ AI = 0;
+ break;
+ }
+
+ /*1.4.4 センサ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);
+ AI = 0;
+ break;
+ }
+
+ /*1.4.5 センサ1-3-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);
+ AI = 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(2.1);
+
+
+
+
+ /********************************/
+ /********************************/
+ /********************************/
+ /*荷物を持ち上げた後のプログラム*/
+ /********************************/
+ /********************************/
+ /********************************/
+
+
+ while(1){
+
+ hoge1 = sw1.read();
+ hoge2 = sw2.read();
+ hoge3 = sw3.read();
+ hoge4 = sw4.read();
+ hoge5 = sw5.read();
+
+
+ /************************************/
+ /*ラインに沿って走るためのプログラム*/
+ /************************************/
+ ///////////まっすぐ走るためのプログラム//////////
+ /*1.1.1 センサ１が黒*/
+ if(sw1 <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
+ motor.Drive(0, CW, 250);
+ motor.Drive(1, CW, 4095);
+
+ }
+
+
+ /*1.1.2 センサ2が黒*/
+ else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
+ motor.Drive(0, CW, 500);
+ motor.Drive(1, CW, 4095);
+
+ }
+
+
+ /*1.1.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);
+
+ }
+
+
+ /*1.1.4 センサ4が黒*/
+ 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, CW, 500);
+
+ }
+
+
+ /*1.1.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, 250);
+
+ }
+
+ /*1.1.6 センサ2-3が黒*/
+ else if(sw1 > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){
+ motor.Drive(0, CW, 1500);
+ motor.Drive(1, CW, 4095);
+
+ }
+
+ /*1.1.7 センサ3-4が黒*/
+ 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, CW, 1500);
+
+ }
+
+ ///////////大きい右カーブを曲がるためのプログラム//////////
+ /*1.2.1 センサ2-5が黒（センサ5によって右のカーブであることを判定する）*/
+ else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 &&sw5 <= 0.3){
+ AI = 1;
+ }
+
+ /*1.2.2 センサ3-5が黒（センサ5によって右のカーブであることを判定する）*/
+ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 <= 0.3){
+ AI = 1;
+ }
+ /*1.2.3 センサ4-5が黒（センサ5によって右のカーブであることを判定する）*/
+ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 <= 0.3){
+ AI = 1;
+ }
+
+ /*1.2..3 センサ2-3-5が黒（センサ5によって右のカーブであることを判定する）*/
+ else if(sw1 > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.3){
+ AI = 1;
+ }
+
+ /*1.2.4 センサ3-4-5が黒（センサ5によって右のカーブであることを判定する）*/
+ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 <= 0.3 && sw5 <= 0.3){
+ AI = 1;
+ }
+
+ /*1.2.5 全部白かつ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, 4095);
+ motor.Drive(1, CCW, 4095);
+
+ }
+
+ ///////////大きい左カーブを曲がるためのプログラム//////////
+ /*1.3.1 センサ1-2が黒（センサ1によって右のカーブであることを判定する）*/
+ else if(sw1 <= 0.3 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
+ AI = 2;
+ }
+
+ /*1.3.2 センサ1-3が黒（センサ1によって右のカーブであることを判定する）*/
+ else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){
+ AI = 2;
+ }
+
+ /*1.3.3 センサ1-4が黒（センサ1によって右のカーブであることを判定する）*/
+ else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 > 0.5){
+ AI = 2;
+ }
+
+ /*1.3.4 センサ1-2-3が黒（センサ1によって右のカーブであることを判定する）*/
+ else if(sw1 <= 0.3 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){
+ AI = 2;
+ }
+
+ /*1.3.5 センサ1-3が黒（センサ1によって右のカーブであることを判定する）*/
+ else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){
+ AI = 2;
+ }
+
+ /*1.3.6 全部白かつ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, 4095);
+ motor.Drive(1, CW, 4095);
+
+ }
+
+ /**********************************/
+ /*停止位置で止まるためのプログラム*/
+ /**********************************/
+
+ /*1.4.1 センサ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);
+ AI = 0;
+ break;
+
+ }
+
+ /*1.4.2 センサ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);
+ AI = 0;
+ break;
+
+ }
+
+ /*1.4.3 センサ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);
+ AI = 0;
+ break;
+ }
+
+ /*1.4.4 センサ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);
+ AI = 0;
+ break;
+ }
+
+ /*1.4.5 センサ1-3-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);
+ AI = 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