center.cpp

00001 #include "mbed.h"
00002 #include "MotorDriver_SU.h"
00003  
00004 DigitalIn sw(P0_1, PullUp); 
00005 // trace L to R
00006 DigitalIn sensor1(P0_16);
00007 DigitalIn sensor2(P0_13);
00008 DigitalIn sensor3(P0_11);
00009 // lift L to R
00010 DigitalIn sensor4(P0_21);
00011 DigitalIn sensor5(P0_22);
00012 
00013 DigitalOut led(P1_28);
00014  
00015 MotorDriver_SU motor(MOTOR_DOUBLE);
00016 MotorDriver_SU lift(MOTOR_SINGLE);
00017  
00018 int main(){
00019     while(1){
00020         
00021     //int height = 0;//lift position
00022     int check1 = 0;//convert count
00023     int check2 = 0;
00024     int count = 0;//count liftsensor B&B
00025     while(sw == 0){
00026                 motor.Drive(0, STOP, 0); //R
00027                 motor.Drive(1, STOP, 0);//L
00028                 lift.Drive(0, STOP, 0);
00029         }
00030 //////////////////////////////////////////////////////////        
00031         //need to change some numerical value after test
00032 //////////////////////////////////////////////////////////
00033     while(sw == 1){
00034             if(sensor1 == 1 && sensor2 == 1 && sensor3 == 0){
00035                 motor.Drive(0, CW, 500);
00036                 motor.Drive(1, CW, 1500);
00037                 }
00038             else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor4 == 1 && sensor5 == 1){
00039                 motor.Drive(0, CW, 1000);
00040                 motor.Drive(1, CW, 1000);
00041                 lift.Drive(0, STOP, 0);
00042                 }
00043             else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor4 == 0 && sensor5 == 0){
00044                 if( count == 0){
00045                     lift.Drive(0, CW, 4095); //lift up
00046                     motor.Drive(0, STOP, 0); //stop
00047                     motor.Drive(1, STOP, 0);
00048                     wait(1);
00049                     motor.Drive(0, CW, 1000); //restart
00050                     motor.Drive(1, CW, 1000); 
00051                     lift.Drive(0, CW, 4095); //lift up more
00052                     wait(0.7);
00053                     lift.Drive(0, STOP, 0);
00054                     //height = 1;//lift position high
00055                     count++;
00056                     }
00057                     //need to change count[1,2,3...] after test
00058                     else if(count == 1){
00059                             count++;
00060                             }
00061                     else if(count == 2){
00062                             count++;
00063                             }
00064                     else if(count == 3){
00065                     motor.Drive(0, STOP, 0);
00066                     motor.Drive(1, STOP, 0);
00067                     lift.Drive(0, CCW, 2000);//lift down
00068                     wait(1.3);
00069                     motor.Drive(0, CCW, 4095);//back for 2 seconds
00070                     motor.Drive(1, CCW, 4095);
00071                     lift.Drive(0, STOP, 0);
00072                     wait(2);
00073                     //height = 0;//lift position low
00074                     count = 0;//reset
00075                     }
00076                     else{
00077                             ;
00078                         }
00079                 }
00080             else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 1){
00081                 motor.Drive(0, CW, 1500);
00082                 motor.Drive(1, STOP, 500);
00083                 }
00084             else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 1){
00085                 motor.Drive(0, CW, 1500);
00086                 motor.Drive(1, CW, 500);
00087                 }
00088             else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 0){
00089                 motor.Drive(0, CW, 500);
00090                 motor.Drive(1, CW, 1500);
00091                 }
00092             else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 0){
00093                 motor.Drive(0, CCW, 500);
00094                 motor.Drive(1, CCW, 500);
00095                 }
00096             /*else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1){
00097                 motor.Drive(0, CW, 2048);
00098                 motor.Drive(1, STOP, 0);
00099                 lift.Drive(0, STOP, 0);
00100                 }*/
00101         //all body on line's outside
00102             else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 ){
00103                     motor.Drive(0, CCW, 200);
00104                     motor.Drive(1, CW, 2000);
00105                     lift.Drive(0, STOP, 0);
00106                     }
00107         //select course condition
00108             else if(sensor2 == 0 && sensor4 == 1 && sensor5 == 0){
00109                         if(check1 == 0){
00110                             check1 = 1;
00111                         //enter the right courses
00112                             motor.Drive(0, CW, 500);//change both speed
00113                             motor.Drive(1, CW, 1500);
00114                             wait(1);
00115                             }
00116                         else{
00117                                 ;//response only once
00118                             }
00119                     }
00120             else if(sensor2 == 0 && sensor4 == 0 && sensor5 == 1){
00121                         if(check2 == 0){
00122                             check2 = 1;
00123                     //enter the center course
00124                             motor.Drive(0, CW, 1500);//change both speed
00125                             motor.Drive(1, CW, 500);
00126                             wait(1);
00127                             }
00128                         else{
00129                                 ;//response only once
00130                                 }
00131                     }
00132             else{
00133                 motor.Drive(0, CW, 2500);//spin to search correct way
00134                 motor.Drive(1, CW, 500);
00135                 }
00136             }
00137     }            
00138 }