masamitsu shibata
a
Dependencies: MotorDriver_SU mbed
Diff: line5.cpp
- Revision:
- 0:b9f6821eb6b5
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/line5.cpp Mon Nov 17 07:02:19 2014 +0000
@@ -0,0 +1,225 @@
+#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 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
+ 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
+ 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 *
+ 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
+ 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
+ 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
+ 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
+ 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
+ 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;
+ }
+ }
+ }
+}
\ No newline at end of file