masamitsu shibata
a
Dependencies: MotorDriver_SU mbed
line8.cpp
- Committer:
- 12f2025e
- Date:
- 2014-11-28
- Revision:
- 0:ac03a5309047
File content as of revision 0:ac03a5309047:
#include "mbed.h"
#include "MotorDriver_SU.h"
DigitalIn sw(P0_1, PullUp);
DigitalIn rightline(P0_12);
DigitalIn leftline(P0_14);
DigitalIn rightlift(P0_22);
DigitalIn leftlift(P0_23);
DigitalOut led1(P1_20); //left
DigitalOut led2(P1_27);
DigitalOut led3(P1_21);
DigitalOut led4(P1_26); //right
MotorDriver_SU motor(MOTOR_DOUBLE);
MotorDriver_SU lift(MOTOR_SINGLE);
int a = 0;
int b = 0;
int c = 0;
int main(){
while(1){
while(sw == 0){
motor.Drive(0, STOP, 0);
motor.Drive(1, STOP, 0);
lift.Drive(0, STOP, 0);
}
while(sw == 1){
if((rightline == 0 && leftline == 1) && (rightlift == 1 && leftlift == 1)){ //WWBW
motor.Drive(0, CW, 2200);
motor.Drive(1, CW, 2200);
lift.Drive(0, STOP, 0);
led1 = 0;
led2 = 0;
led3 = 1;
led4 = 0;
}
else if((rightline == 0 && leftline == 0) && (rightlift == 0 && leftlift == 0)){ //BBBB
if(a == 0){
led1 = 1;
led2 = 1;
led3 = 1;
led4 = 1;
motor.Drive(0, CW, 2000); //0 = rightmotor, 1 = leftmotor
motor.Drive(1, CW, 2000);
lift.Drive(0, CCW, 4095);
wait(1.5);
lift.Drive(0, STOP, 0);
a = 1;
}
else{
led1 = 1;
led2 = 1;
led3 = 1;
led4 = 1;
motor.Drive(0, STOP, 0);
motor.Drive(1, STOP, 0);
lift.Drive(0, CW, 4095);
wait(1.5);
lift.Drive(0, STOP, 0);
motor.Drive(0, CCW, 4095);
motor.Drive(1, CCW, 4095);
wait(1);
}
}
else if((rightline == 0 && leftline == 0) && (rightlift == 1 && leftlift == 0)){ //BBBW
if(c == 0){
led1 = 1;
led2 = 1;
led3 = 1;
led4 = 0;
motor.Drive(0, CW, 1000);
motor.Drive(1, CW, 1000);
wait(0.7);
motor.Drive(0, CW, 1000);
motor.Drive(1, CCW, 1000);
wait(0.1);
c = 1;
}
else{
motor.Drive(0, CW, 2200);
motor.Drive(1, STOP, 0);
lift.Drive(0, STOP, 0);
led1 = 1;
led2 = 1;
led3 = 1;
led4 = 0;
}
}
else if((rightline == 0 && leftline == 0) && (rightlift == 0 && leftlift == 1)){ //WBBB
if(b == 0){
led1 = 0;
led2 = 1;
led3 = 1;
led4 = 1;
motor.Drive(0, CW, 1000);
motor.Drive(1, CW, 1000);
wait(1);
motor.Drive(0, CCW, 1000);
motor.Drive(1, CW, 1000);
wait(0.5);
b = 1;
}
else{
motor.Drive(0, STOP, 0);
motor.Drive(1, CW, 2200);
lift.Drive(0, STOP, 0);
led1 = 0;
led2 = 1;
led3 = 1;
led4 = 1;
}
}
else if((rightline == 0 && leftline == 0) && (rightlift == 1 && leftlift == 1)){ //WBBW *
motor.Drive(0, CW, 2200);
motor.Drive(1, CW, 1100);
lift.Drive(0, STOP, 0);
led1 = 0;
led2 = 1;
led3 = 1;
led4 = 0;
}
else if((rightline == 1 && leftline == 1) && (rightlift == 1 && leftlift == 0)){ //BWWW *
if(c == 0){
led1 = 1;
led2 = 0;
led3 = 0;
led4 = 0;
motor.Drive(0, CW, 1000);
motor.Drive(1, CW, 1000);
wait(0.7);
motor.Drive(0, CW, 1000);
motor.Drive(1, CCW, 1000);
wait(0.1);
motor.Drive(0, CW, 1000);
motor.Drive(1, CW, 1000);
wait(0.3);
c = 1;
}
else{
motor.Drive(0, CW, 1500);
motor.Drive(1, CW, 1000);
lift.Drive(0, STOP, 0);
led1 = 1;
led2 = 0;
led3 = 0;
led4 = 0;
}
}
else if((rightline == 1 && leftline == 1) && (rightlift == 0 && leftlift == 1)){ //WWWB
if(b == 0){
led1 = 0;
led2 = 0;
led3 = 0;
led4 = 1;
motor.Drive(0, CW, 1000);
motor.Drive(1, CW, 1000);
wait(1);
motor.Drive(0, CCW, 1000);
motor.Drive(1, CW, 1000);
wait(0.5);
b = 1;
}
else{
motor.Drive(0, CW, 500);
motor.Drive(1, CW, 1500);
lift.Drive(0, STOP, 0);
led1 = 0;
led2 = 0;
led3 = 0;
led4 = 1;
}
}
else if((rightline == 1 && leftline == 1) && (rightlift == 1 && leftlift == 1)){ //WWWW
motor.Drive(0, CCW, 650);
motor.Drive(1, CW, 1500);
lift.Drive(0, STOP, 0);
led1 = 0;
led2 = 0;
led3 = 0;
led4 = 0;
}
else if((rightline == 0 && leftline == 1) && (rightlift == 0 && leftlift == 0)){ //BWBB
if(a == 0){
led1 = 1;
led2 = 0;
led3 = 1;
led4 = 1;
motor.Drive(0, CW, 2000);
motor.Drive(1, CW, 2000);
lift.Drive(0, CCW, 4095);
wait(1.5);
lift.Drive(0, STOP, 0);
a = 1;
}
else{
led1 = 1;
led2 = 0;
led3 = 1;
led4 = 1;
motor.Drive(0, STOP, 0);
motor.Drive(1, STOP, 0);
lift.Drive(0, CW, 4095);
wait(1.5);
lift.Drive(0, STOP, 0);
motor.Drive(0, CCW, 4095);
motor.Drive(1, CCW, 4095);
wait(1);
}
}
else if((rightline == 0 && leftline == 1) && (rightlift == 1 && leftlift == 0)){ //BWBW
if(c == 0){
led1 = 1;
led2 = 0;
led3 = 1;
led4 = 0;
motor.Drive(0, CW, 1000);
motor.Drive(1, CW, 1000);
wait(0.7);
motor.Drive(0, CW, 1000);
motor.Drive(1, CCW, 1000);
wait(0.1);
c = 1;
}
else{
motor.Drive(0, CW, 2200);
motor.Drive(1, STOP, 0);
lift.Drive(0, STOP, 0);
led1 = 1;
led2 = 0;
led3 = 1;
led4 = 0;
}
}
else if((rightline == 0 && leftline == 1) && (rightlift == 0 && leftlift == 1)){ //WWBB
if(b == 0){
led1 = 0;
led2 = 0;
led3 = 1;
led4 = 1;
motor.Drive(0, CW, 1000);
motor.Drive(1, CW, 1000);
wait(1);
motor.Drive(0, CCW, 1000);
motor.Drive(1, CW, 1000);
wait(0.5);
b = 1;
}
else{
motor.Drive(0, CW, 1000);
motor.Drive(1, CW, 1500);
lift.Drive(0, STOP, 0);
led1 = 0;
led2 = 0;
led3 = 1;
led4 = 1;
}
}
else if((rightline == 1 && leftline == 0) && (rightlift == 0 && leftlift == 0)){ //BBWB
if(a == 0){
led1 = 1;
led2 = 1;
led3 = 0;
led4 = 1;
motor.Drive(0, CW, 2000);
motor.Drive(1, CW, 2000);
lift.Drive(0, CCW, 4095);
wait(1.5);
lift.Drive(0, STOP, 0);
a = 1;
}
else{
led1 = 1;
led2 = 1;
led3 = 0;
led4 = 1;
motor.Drive(0, STOP, 0);
motor.Drive(1, STOP, 0);
lift.Drive(0, CW, 4095);
wait(1.5);
lift.Drive(0, STOP, 0);
motor.Drive(0, CCW, 4095);
motor.Drive(1, CCW, 4095);
wait(1);
}
}
else if((rightline == 1 && leftline == 0) && (rightlift == 1 && leftlift == 0)){ //BBWW
if(c == 0){
led1 = 1;
led2 = 1;
led3 = 0;
led4 = 0;
motor.Drive(0, CW, 1000);
motor.Drive(1, CW, 1000);
wait(0.7);
motor.Drive(0, CW, 1000);
motor.Drive(1, CCW, 1000);
wait(0.1);
c = 1;
}
else{
motor.Drive(0, CW, 2200);
motor.Drive(1, STOP, 0);
lift.Drive(0, STOP, 0);
led1 = 1;
led2 = 1;
led3 = 0;
led4 = 0;
}
}
else if((rightline == 1 && leftline == 0) && (rightlift == 0 && leftlift == 1)){ //WBWB
if(b == 0){
led1 = 0;
led2 = 1;
led3 = 0;
led4 = 1;
motor.Drive(0, CW, 1000);
motor.Drive(1, CW, 1000);
wait(1);
motor.Drive(0, CCW, 1000);
motor.Drive(1, CW, 1000);
wait(0.5);
b = 1;
}
else{
motor.Drive(0, STOP, 0);
motor.Drive(1, CW, 2200);
lift.Drive(0, STOP, 0);
led1 = 0;
led2 = 1;
led3 = 0;
led4 = 1;
}
}
else{ //WBWW
motor.Drive(0, CW, 2200);
motor.Drive(1, CW, 1000);
lift.Drive(0, STOP, 0);
led1 = 0;
led2 = 1;
led3 = 0;
led4 = 0;
}
}
}
}