Diff: main/main.cpp

Revision:

1:79219d0a33c8

Parent:

0:df5216b20861

Child:

2:940e46e21353

diff -r df5216b20861 -r 79219d0a33c8 main/main.cpp
--- a/main/main.cpp	Tue Feb 04 19:18:10 2020 +0000
+++ b/main/main.cpp	Wed Feb 05 19:57:48 2020 +0000
@@ -24,8 +24,13 @@
 void init();
 void calibrate();
 void follow_line( float speed );
+char junction_detect( unsigned int* sensor );
+void turn_selector( char turn );
 void left();
+void right();
 void back();
+//void goal( char* simple_path );
+//char* simplify( char* path, int path_length );
 
 // Constants
 
@@ -46,11 +51,15 @@
     float integral = 0.0;
     float derivative = 0.0;
     
+    char path[100];
+    int path_length = 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;
@@ -76,10 +85,19 @@
         
         //follow_line(speed);
         
-        if (sensor[0] > 500) {
-            left();
-        }else if(sensor[0] < 500 && sensor[1] < 500 && sensor[2] < 500 && sensor[3] < 500 && sensor[4] < 500) {
-            back();
+        char turn = junction_detect(sensor);
+        turn_selector(turn);
+        
+        if ( turn != 'S' ) {
+            path[path_length] = turn;
+            path_length ++;
+        }
+        
+        if ( turn == 'G' ) {
+            //char simple_path[100];
+            //simple_path = simplify( path, path_length );
+            
+            //goal(simple_path);  // make this an infinite loop that 
         }
         
         robot.display_data();
@@ -117,22 +135,62 @@
       
 void follow_line( float speed ) 
 {
-    float position = robot.read_line();
 
-    float correction = position * speed;
+}
 
-    if (position>0) {
-        robot.motors(speed, speed-correction);
+char junction_detect( unsigned int* sensor ) 
+{
+    // add a goal detector
+    if ( sensor[0] < 500 && sensor[1] < 500 && sensor[2] < 500 && sensor[3] < 500 && sensor[4] < 500 ) {
+        return 'B';
+    } else 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 {
-        robot.motors(speed+correction,speed);
+        return 'S';
     }
 }
 
+void turn_selector( char turn )
+{
+    switch(turn) {
+        //case 'G':
+        //    goal();
+        case 'L':
+            left();
+        case 'S':
+            break;
+        case 'R':
+            right();
+        case 'B':
+            back();
+    }
+}
+        
 void left()
 {
+    leds = 0b1100;
     //while (sensor[0] > 500) { robot.scan(); }
     wait(0.2);
     robot.spin_left(0.2);
+    wait(0.32);                              // this wait could be replaced with while statements that wait for sensor3 to become untrigger then retriggered)
+    /*while (sensor[3] > 500) {
+        while (sensor[3] < 500) {
+        }
+    }*/
+    //robot.scan();
+    //robot.follow_line();
+}
+
+void right()
+{
+    leds = 0b0011;
+    //while (sensor[0] > 500) { robot.scan(); }
+    wait(0.2);
+    robot.spin_right(0.2);
     wait(0.32);
     //robot.scan();
     //robot.follow_line();
@@ -140,9 +198,20 @@
 
 void back() 
 {
+    leds = 0b1111;
     robot.reverse(0.2);
     wait(0.1);
     robot.spin_right(0.2);
     wait(0.6);
 }
+
+void goal( char* simple_path )
+{
+
+}
+
+char* simplify( char* path, int path_length )
+{
+    
+}
     
\ No newline at end of file