Diff: center.cpp

Revision:

0:cc0c03ee56b3

Child:

1:0cd50a576350

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/center.cpp	Wed Nov 19 13:08:25 2014 +0000
@@ -0,0 +1,138 @@
+#include "mbed.h"
+#include "MotorDriver_SU.h"
+ 
+DigitalIn sw(P0_1, PullUp); 
+// trace L to R
+DigitalIn sensor1(P0_16);
+DigitalIn sensor2(P0_13);
+DigitalIn sensor3(P0_11);
+// lift L to R
+DigitalIn sensor4(P0_21);
+DigitalIn sensor5(P0_22);
+
+DigitalOut led(P1_28);
+ 
+MotorDriver_SU motor(MOTOR_DOUBLE);
+MotorDriver_SU lift(MOTOR_SINGLE);
+ 
+int main(){
+    while(1){
+        
+    //int height = 0;//lift position
+    int check1 = 0;//convert count
+    int check2 = 0;
+    int count = 0;//count liftsensor B&B
+    while(sw == 0){
+                motor.Drive(0, STOP, 0); //R
+                motor.Drive(1, STOP, 0);//L
+                lift.Drive(0, STOP, 0);
+        }
+//////////////////////////////////////////////////////////        
+        //need to change some numerical value after test
+//////////////////////////////////////////////////////////
+    while(sw == 1){
+            if(sensor1 == 1 && sensor2 == 1 && sensor3 == 0){
+                motor.Drive(0, CW, 500);
+                motor.Drive(1, CW, 1500);
+                }
+            else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor4 == 1 && sensor5 == 1){
+                motor.Drive(0, CW, 1000);
+                motor.Drive(1, CW, 1000);
+                lift.Drive(0, STOP, 0);
+                }
+            else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor4 == 0 && sensor5 == 0){
+                if( count == 0){
+                    lift.Drive(0, CW, 4095); //lift up
+                    motor.Drive(0, STOP, 0); //stop
+                    motor.Drive(1, STOP, 0);
+                    wait(1);
+                    motor.Drive(0, CW, 1000); //restart
+                    motor.Drive(1, CW, 1000); 
+                    lift.Drive(0, CW, 4095); //lift up more
+                    wait(0.7);
+                    lift.Drive(0, STOP, 0);
+                    //height = 1;//lift position high
+                    count++;
+                    }
+                    //need to change count[1,2,3...] after test
+                    else if(count == 1){
+                            count++;
+                            }
+                    else if(count == 2){
+                            count++;
+                            }
+                    else if(count == 3){
+                    motor.Drive(0, STOP, 0);
+                    motor.Drive(1, STOP, 0);
+                    lift.Drive(0, CCW, 2000);//lift down
+                    wait(1.3);
+                    motor.Drive(0, CCW, 4095);//back for 2 seconds
+                    motor.Drive(1, CCW, 4095);
+                    lift.Drive(0, STOP, 0);
+                    wait(2);
+                    //height = 0;//lift position low
+                    count = 0;//reset
+                    }
+                    else{
+                            ;
+                        }
+                }
+            else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 1){
+                motor.Drive(0, CW, 1500);
+                motor.Drive(1, STOP, 500);
+                }
+            else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 1){
+                motor.Drive(0, CW, 1500);
+                motor.Drive(1, CW, 500);
+                }
+            else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 0){
+                motor.Drive(0, CW, 500);
+                motor.Drive(1, CW, 1500);
+                }
+            else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 0){
+                motor.Drive(0, CCW, 500);
+                motor.Drive(1, CCW, 500);
+                }
+            /*else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1){
+                motor.Drive(0, CW, 2048);
+                motor.Drive(1, STOP, 0);
+                lift.Drive(0, STOP, 0);
+                }*/
+        //all body on line's outside
+            else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 ){
+                    motor.Drive(0, CCW, 200);
+                    motor.Drive(1, CW, 2000);
+                    lift.Drive(0, STOP, 0);
+                    }
+        //select course condition
+            else if(sensor2 == 0 && sensor4 == 1 && sensor5 == 0){
+                        if(check1 == 0){
+                            check1 = 1;
+                        //enter the right courses
+                            motor.Drive(0, CW, 500);//change both speed
+                            motor.Drive(1, CW, 1500);
+                            wait(1);
+                            }
+                        else{
+                                ;//response only once
+                            }
+                    }
+            else if(sensor2 == 0 && sensor4 == 0 && sensor5 == 1){
+                        if(check2 == 0){
+                            check2 = 1;
+                    //enter the center course
+                            motor.Drive(0, CW, 1500);//change both speed
+                            motor.Drive(1, CW, 500);
+                            wait(1);
+                            }
+                        else{
+                                ;//response only once
+                                }
+                    }
+            else{
+                motor.Drive(0, CW, 2500);//spin to search correct way
+                motor.Drive(1, CW, 500);
+                }
+            }
+    }            
+}
\ No newline at end of file