Diff: main/main.cpp

Revision:

10:691c02b352cb

Parent:

9:952586accbf9

Child:

11:c3299aca7d8f

--- a/main/main.cpp	Sat Feb 22 05:01:27 2020 +0000
+++ b/main/main.cpp	Mon Feb 24 04:51:39 2020 +0000
@@ -23,17 +23,15 @@
 // Prototypes
 void init();
 void calibrate();
-void follow_line( float speed );
+void follow_line();
 char junction_detect();
-char turn_logic();
 void turn_selector( char turn );
 void left();
 void right();
 void back();
-//void check_goal();
 void goal();
 void simplify();
-void return_to_start();
+void invert_path();
 
 // Constants
 
@@ -47,10 +45,14 @@
 // Global Variables
 
 char path[100];
+char inv_path[100];
 int path_length = 0;
 unsigned int *sensor;  
 float speed;
-int goal_check;
+float proportional = 0.0;
+float prev_proportional = 0.0;
+float integral = 0.0;
+float derivative = 0.0;
 
 // Main
 
@@ -60,61 +62,18 @@
     calibrate();
     speed = (pot_S*0.3)+0.2;   // have it so max is 0.5 and min is 0.2
     
-    float proportional = 0.0;
-    float prev_proportional = 0.0;
-    float integral = 0.0;
-    float derivative = 0.0;
-    
     float dt = 1/50;           // updating 50 times a second
     
     while (1) {
-        robot.scan();
-    
-        sensor = robot.get_sensors(); // returns the current values of all the sensors from 0-1000
-        leds = 0b0110;
-        
-        proportional = robot.read_line();  // returns a value between -1,1     (-1 = PC0 or further , -1 to -0.5 = PC1 (-0.5 is directly below PC1) , -0.5 to 0 = PC2 , 0 to 0.5 = PC3 , 0.5 to 1 and further = PC4)
-        derivative = proportional - prev_proportional;
-        integral += proportional;
-        prev_proportional = proportional;
-        
-        // calculate motor correction
-        float motor_correction = proportional*A + integral*B + derivative*C;
-        
-        // make sure the correction is never greater than the max speed as the motor will reverse
-        if( motor_correction > speed ) {
-            motor_correction = speed;
-        }
-        if( motor_correction < -speed ) {
-            motor_correction = -speed;
+
+        follow_line();
+
+        char turn = junction_detect(); // rename this function something else
+        turn_selector(turn);
+        if ( turn != 'S' ) {              // doesnt need 'S', also may not need 'R' as that is not technically a junction
+            path[path_length] = turn;
+            path_length ++;
         }
-    
-        if( proportional < 0 ) {
-            robot.motors(speed+motor_correction,speed);
-        } else {
-            robot.motors(speed,speed-motor_correction);
-        }
-        
-        //follow_line(speed);
-        if ( sensor[0] > sens_thresh || sensor[4] > sens_thresh ) {
-            wait(0.05);
-            if ( sensor[0] > sens_thresh && sensor[4] > sens_thresh && sensor[1] < sens_thresh && sensor[2] < sens_thresh && sensor[3] < sens_thresh ) {
-                goal();
-            }
-        }
-        // add if juntion detected, move forward until both 0 and 4 or the middle three are low, then check the type of junction and turn accordingly (lines below)
-        //if ( junction_detect() == true ){
-            char turn = junction_detect(); // rename this function something else
-            turn_selector(turn);
-            if ( turn != 'S' && turn != 'R') {              // doesnt need 'S', also may not need 'R' as that is not technically a junction
-                path[path_length] = turn;
-                path_length ++;
-            }
-        
-        //}
-        
-        
- 
         
         simplify();
         
@@ -138,7 +97,7 @@
     button_enter.mode(PullUp);
     button_back.mode(PullUp);
 
-    //leds = 0b0000;
+    leds = 0b0000;
 }
 
 void calibrate()
@@ -154,9 +113,32 @@
     leds = 0b0000;
 }
       
-void follow_line( float speed ) 
+void follow_line() 
 {
+    robot.scan();
+    sensor = robot.get_sensors(); // returns the current values of all the sensors from 0-1000
+    
+    proportional = robot.read_line();  // returns a value between -1,1     (-1 = PC0 or further , -1 to -0.5 = PC1 (-0.5 is directly below PC1) , -0.5 to 0 = PC2 , 0 to 0.5 = PC3 , 0.5 to 1 and further = PC4)
+    derivative = proportional - prev_proportional;
+    integral += proportional;
+    prev_proportional = proportional;
+    
+    // calculate motor correction
+    float motor_correction = proportional*A + integral*B + derivative*C;
+    
+    // make sure the correction is never greater than the max speed as the motor will reverse
+    if( motor_correction > speed ) {
+        motor_correction = speed;
+    }
+    if( motor_correction < -speed ) {
+        motor_correction = -speed;
+    }
 
+    if( proportional < 0 ) {
+        robot.motors(speed+motor_correction,speed);
+    } else {
+        robot.motors(speed,speed-motor_correction);
+    }
 }
 
 char junction_detect() 
@@ -213,8 +195,6 @@
         return 'B';
     }
     
-    
-    
     if (goal) {
         return 'G';
     } else if (left) {
@@ -228,25 +208,6 @@
     }
 }
 
-char turn_logic() 
-{
-    
-    // either increase the wait in main loop 50-> 20 
-    
-    //or have it so it inches forward at a junction (if junction detected, then inch forward and decide
-    
-    if ( sensor[0] > 500 ) {
-        return 'L';
-    } else if ( sensor[1] > 500 || sensor[2] > 500 || sensor[3] > 500 ) {
-        return 'S';
-    } else if ( sensor[4] > 500 ) {
-        return 'R';
-    } else if ( sensor[0] < 500 && sensor[1] < 500 && sensor[2] < 500 && sensor[3] < 500 && sensor[4] < 500 ) {
-        return 'B';
-    } else {
-        return 'S';
-    }
-}
 
 void turn_selector( char turn )
 {
@@ -313,13 +274,13 @@
     // if it was, iterate over the last three turns and check the total angle change
     // replace the three turns with the new single turn
     
-    if( path[path_length-2] == 'B' && path_length > 3) {
-        int angle_change;
+    if( path[path_length-2] == 'B' && path_length >= 3) {
+        int angle_change = 0;
         
-        for (int i = 0; i < 3; i++) {
-            if (path[i] == 'L') { angle_change += 270; }
-            else if (path[i] == 'R') { angle_change += 90; }
-            else if (path[i] == 'B') { angle_change += 180; }
+        for (int i = 1; i <= 3; i++) {
+            if (path[path_length - i] == 'L') { angle_change += 270; }
+            else if (path[path_length - i] == 'R') { angle_change += 90; }
+            else if (path[path_length - i] == 'B') { angle_change += 180; }
         }
         
         angle_change = angle_change % 360;
@@ -335,16 +296,44 @@
     }
 }  
 
-/*
-void check_goal() 
-{
-    if (sensor[0] > sens_thresh && sensor[1] > sens_thresh && sensor[2] > sens_thresh && sensor[3] > sens_thresh && sensor[4] > sens_thresh) {
-        goal_check ++;
+void goal()
+{ 
+    invert_path();
+    int pointer = 0;   //path_length - 1
+    
+    robot.lcd_clear();
+    robot.lcd_print(path,100);
+    
+    robot.stop();
+    while (button_enter.read() == 1) {}  // keep looping waiting for Enter to be pressed
+    
+    leds = 0b1001;
+    wait(0.2);
+    leds = 0b0110;
+    wait(0.2);
+    leds = 0b1001;
+    wait(0.2);
+    leds = 0b0110;
+    wait(0.2);
+    
+    //robot.reverse(speed);
+//    back();
+    
+    while(pointer <= path_length) {
+        follow_line();
+        
+        if (sensor[0] > sens_thresh || sensor[4] > sens_thresh) {  // if junction found
+            turn_selector(path[pointer]);
+            if(path[pointer] == 'S') {      // make this better
+                robot.forward(speed);
+                leds = 0b1010;
+                wait(0.3);
+                // while((sensor[0] > sens_thresh || sensor[4] > sens_thresh) { robot.scan(); }
+            }
+            pointer++;
+        }
     }
-}*/
-
-void goal()
-{
+    
     robot.stop();
     while(1) {
         leds = 0b1000;
@@ -358,7 +347,12 @@
     }
 }
 
-void return_to_start() 
+void invert_path()
 {
-    
+    // only call once then can use infinitely
+    for( int i = 0; i <= path_length; i++ ){
+        inv_path[i] = path[path_length-i];
+        if( inv_path[i] == 'L' ) { inv_path[i] = 'R'; }
+        if( inv_path[i] == 'R' ) { inv_path[i] = 'L'; }
+    }
 }
\ No newline at end of file