Revision 3:bebb73b82a17, committed 2015-04-11

Comitter:

Risifutti

Date:

Sat Apr 11 08:10:49 2015 +0000

Parent:

2:39179781a18c

Commit message:

R?GE

Changed in this revision

--- a/main.cpp	Fri Apr 10 23:54:59 2015 +0000
+++ b/main.cpp	Sat Apr 11 08:10:49 2015 +0000
@@ -23,42 +23,74 @@
 }
 
 
+//SELVKØRENDE I MØRKET!
+void selfDrive(){
+    
+    int dLEFT = sensorLEFT.distance(CM);
+    int dRIGHT = sensorRIGHT.distance(CM);
+    
+    // Writes the left and right distance variable
+       pc.printf("SENSOR Left: %d \n\r\v",distLeft(dLEFT));
+       pc.printf("SENSOR Right: %d \n\r\v",distRight(dRIGHT));
+    
+        
+    //Drej til venstre for at rette op
+    if(dRIGHT > 60 && dLEFT > 60 || BT.getc()=='p'){
+      control = 0;
+    }
+    
+    else if(dRIGHT > 35){
+      leftForward = 0.75;
+      leftBackward = 0.25;
+      rightForward = 0.25;
+      rightBackward = 0.75;
+    }
+    
+    else if(dLEFT > 35){
+      leftForward = 0.25;
+      leftBackward = 0.75;
+      rightForward = 0.75;
+      rightBackward = 0.25;
+    }
+    
+    else if(dRIGHT > 50){
+      leftForward = 0.1;
+      leftBackward = 0.9;
+      rightForward = 0.9;
+      rightBackward = 0.1;
+    }     
+    
+    else if(dLEFT > 50){
+      leftForward = 0.1;
+      leftBackward = 0.9;
+      rightForward = 0.9;
+      rightBackward = 0.1;
+      
+    else{
+      leftForward = 1;
+      leftBackward = 0;
+      rightForward = 1;
+      rightBackward = 0;
+    }         
+}     
 // The main() function
 int main()
 {
     
+    
     // The loop() function
     while(1) {
-        //left and right distance variables 
-        int dLEFT = sensorLEFT.distance(CM);
-        int dRIGHT = sensorRIGHT.distance(CM);
+        //left and right distance variables
+                selfControl(dRIGHT, dLEFT);    
         
-        // Writes the left and right distance variable
-       pc.printf("SENSOR Left: %d \n\r\v",distLeft(dLEFT));
-       pc.printf("SENSOR Right: %d \n\r\v",distRight(dRIGHT));
         
         // Delay
         wait(0.5);
-        
-        //LED control
-        if(dRIGHT<=10) {
-            led = 0;
-            led2 = 1;
-        }
-
-        if(dRIGHT>=40) {
-            led2 = 0;
-            led = 1;
-        }
-        if(dRIGHT>=11 && dRIGHT<=39) {
-            led2 = 0;
-            led=0;
-        }
     }
     
 }
 
+ 
 
 
 
-