a

Dependencies:   MotorDriver_SU mbed

Revision:
0:a421b8dde3ab
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/c.cpp	Thu Dec 04 06:49:04 2014 +0000
@@ -0,0 +1,162 @@
+#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 sensor6(P0_14);
+DigitalIn sensor5(P0_22);
+DigitalIn sensor7(P0_23);
+
+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, 2500);
+                motor.Drive(1, CW, 2500);
+                lift.Drive(0, STOP, 0);
+                }
+            else if((sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor4 == 0 && sensor5 == 0)/*||(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor4 == 0 && sensor7 == 0)||(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor6 == 0 && sensor5 == 0)||(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor5 == 0 && sensor7 == 0)*/){
+                led = 1;//count kakuninn
+                if( count == 0){
+                    lift.Drive(0, CW, 4095); //lift up
+                    motor.Drive(0, CW, 1300); //restart
+                    motor.Drive(1, CW, 1300); 
+                    /*motor.Drive(0, STOP, 0); //stop
+                    motor.Drive(1, STOP, 0);*/
+                    wait(1.6);
+                    
+                    led = 0;//count kakuninn
+                   /* motor.Drive(0, CW, 2000); //restart
+                    motor.Drive(1, CW, 2000); 
+                    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 = 3;
+                            }
+                    else if(count == 3){
+                    motor.Drive(0, STOP, 0);
+                    motor.Drive(1, STOP, 0);
+                    lift.Drive(0, CCW, 3000);//lift down
+                    wait(1.2);
+                    motor.Drive(0, CCW, 4095);//back for 2 seconds
+                    motor.Drive(1, CCW, 4095);
+                    lift.Drive(0, STOP, 0);
+                    led = 0;//count kakunin
+                    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, CW, 500);
+                }
+            else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 1){
+                motor.Drive(0, CW, 1875);
+                motor.Drive(1, CW, 1500);
+                }
+            else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 0){
+                motor.Drive(0, CW, 1500);
+                motor.Drive(1, CW, 1875);
+                }
+            else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 0){
+                motor.Drive(0, CW, 500);//ccw  change cw
+                motor.Drive(1, CW, 500);//ccw  change cw
+                }
+            /*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 )||(sensor2 == 0 && sensor4 == 1 && sensor7 == 0 )||(sensor2 == 0 && sensor6 == 1 && sensor5 == 0)||(sensor2 == 0 && sensor6 == 1 && sensor7 == 0)){
+                        if(check1 == 0){
+                            check1 = 1;
+                            motor.Drive(0, CW, 1500);//adjust
+                            motor.Drive(1, CW, 1500);
+                           wait(0.5);//adjust
+                        //enter the right courses
+                            motor.Drive(0, CW, 500);//change both speed
+                            motor.Drive(1, CW, 2500);
+                            wait(1);
+                            motor.Drive(0, CW, 1500);//adjust
+                            motor.Drive(1, CW, 1500);
+                           wait(0.15);//adjust
+                           motor.Drive(0, CW, 1200);//change both speed
+                           motor.Drive(1, CCW, 200);
+                            wait(0.6);
+                            /*motor.Drive(0, CW, 1500);//adjust
+                            motor.Drive(1, CW, 1500);
+                           wait(0.1);//adjust*/
+                            }
+                        else{
+                                ;//response only once
+                            }
+                    }
+            /*else if((sensor2 == 0 && sensor4 == 0 && sensor5 == 1)||(sensor2 == 0 && sensor4 == 0 && sensor7 == 1)||(sensor2 == 0 && sensor6 == 0 && sensor5 == 1)||(sensor2 == 0 && sensor6 == 0 && sensor7 == 1)){
+                        if(check2 == 0){
+                            check2 = 1;
+                    //enter the center course
+                            motor.Drive(0, CW, 800);//change both speed
+                            motor.Drive(1, CCW, 500);
+                            wait(1);
+                            }
+            else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 0 ) {
+                motor.Drive(0, CW, 500);
+                motor.Drive(1, CW, 500);
+                }//tuika
+                         
+                            
+                        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