圭祐 杉浦
本番(11/10)で使用したプログラム
Dependencies: MotorDriver_SU USBDevice mbed
termB-1.cpp
- Committer:
- 12f2027a
- Date:
- 2014-11-10
- Revision:
- 0:29e77dab275d
File content as of revision 0:29e77dab275d:
#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;
/********************************/
/********************************/
/********************************/
/*荷物を持ち上げる前のプログラム*/
/********************************/
/********************************/
/********************************/
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, 500);
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, 800);
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, 800);
}
/*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, 500);
}
/*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);
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);
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);
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);
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);
break;
}
}
///////////停止//////////
motor.Drive(0, STOP, 0);
motor.Drive(1, STOP, 0);
wait(0.1);
///////////リフト上げる//////////
lift.Drive(0, CCW, 4095);
wait(0.8);
///////////リフト停止//////////
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.1 センサ１が黒*/
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, 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, 800);
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, 800);
}
/*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, 500);
}
/*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);
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);
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);
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);
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);
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(3.0);
///////////モータ停止//////////
motor.Drive(0, STOP, 0);
motor.Drive(1, STOP, 0);
wait(1.0);
}