圭祐 杉浦
termB 2回目 本番用
Dependencies: MotorDriver_SU USBDevice mbed
Diff: termB-2.cpp
- Revision:
- 0:a66927a110ab
- Child:
- 1:1efcac695408
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/termB-2.cpp Sun Nov 16 17:45:56 2014 +0000
@@ -0,0 +1,210 @@
+#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;
+ int AI = 0;
+ int updown = 0;
+
+ while(1){
+
+ hoge1 = sw1.read();
+ hoge2 = sw2.read();
+ hoge3 = sw3.read();
+ hoge4 = sw4.read();
+ hoge5 = sw5.read();
+
+
+ /************************************/
+ /*ラインに沿って走るためのプログラム*/
+ /************************************/
+ ///////////まっすぐ走るためのプログラム//////////
+ /*黒＿白＿白＿白＿白*/
+ if(sw1 <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
+ motor.Drive(0, CW, 500);
+ motor.Drive(1, CW, 2000);
+
+ }
+
+
+ /*白＿黒＿白＿白＿白*/
+ else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
+ motor.Drive(0, CW, 800);
+ motor.Drive(1, CW, 2000);
+
+ }
+ /*白＿黒＿黒＿白＿白*/
+ else if(sw1 > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){
+ motor.Drive(0, CW, 1000);
+ motor.Drive(1, CW, 2000);
+
+ }
+
+
+ /*白＿白＿黒＿白＿白*/
+ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){
+ motor.Drive(0, CW, 1600);
+ motor.Drive(1, CW, 1600);
+
+ }
+ /*白＿白＿黒＿黒＿白*/
+ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){
+ motor.Drive(0, CW, 2000);
+ motor.Drive(1, CW, 1000);
+
+ }
+
+
+
+ /*白＿白＿白＿黒＿白*/
+ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.5 && sw5 > 0.5){
+ motor.Drive(0, CW, 2000);
+ motor.Drive(1, CW, 800);
+
+ }
+
+
+ /*白＿白＿白＿白＿黒*/
+ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 <= 0.3){
+ motor.Drive(0, CW, 2000);
+ motor.Drive(1, CW, 500);
+
+ }
+
+
+
+
+ ///////////大きい右カーブを曲がるためのプログラム//////////
+ /*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, 1800);
+ motor.Drive(1, CCW, 1800);
+
+ }
+
+ ///////////大きい左カーブを曲がるためのプログラム//////////
+ /*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, 1800);
+ motor.Drive(1, CW, 1800);
+
+ }
+
+ /**********************************/
+ /*停止位置で止まるためのプログラム*/
+ /**********************************/
+
+ /*1.4.1 センサ1-5が黒*/
+ else if(sw1 <= 0.5 && sw5 <= 0.5){
+ motor.Drive(0, STOP, 0);
+ motor.Drive(1, STOP, 0);
+ if( updown == 0 ){
+ ///////////停止//////////
+ motor.Drive(0, STOP, 0);
+ motor.Drive(1, STOP, 0);
+ wait(0.1);
+ ///////////リフト上げる//////////
+ lift.Drive(0, CCW, 4095);
+ wait(1.0);
+ ///////////リフト停止//////////
+ lift.Drive(0, STOP, 0);
+ wait(0.1);
+ ///////////停止位置を超えるために少し前進//////////
+ motor.Drive(0, CW, 1500);
+ motor.Drive(1, CW, 1500);
+ wait(1.0);
+ 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(3.0);
+ ///////////モータ停止//////////
+ motor.Drive(0, STOP, 0);
+ motor.Drive(1, STOP, 0);
+ wait(1.0);
+ updown = 0;
+ break;
+ }
+ }
+
+}
+}
\ No newline at end of file