Diff: main/main.cpp

Revision:

21:54ea75f7984f

Parent:

20:5cf6a378801d

Child:

22:02dda79d50b4

--- a/main/main.cpp	Tue Mar 24 14:20:32 2020 +0000
+++ b/main/main.cpp	Fri Apr 03 19:55:09 2020 +0000
@@ -57,42 +57,7 @@
     float dt = 1/50;           // updating 50 times a second
 
     while (1) {
-        // encoder testing
-//        speed = (pot_S);   // have it so max is 0.5 and min is 0.2 (this lowest doesnt work)
-//        read_encoders();     
-//        if (encoder[0] > 1100) { 
-//            
-//            leds[1] = 1;
-//        }
-//        else { 
-//            leds[1] = 0;
-//            dist_est_1 += 1;
-//        }  // going to have to reset these dist estimates every junction (in the if (junc_detect()) statement)
-//        if (encoder[1] > 1100) { // black
-//             
-//            leds[2] = 1;
-//        }
-//        else { 
-//            leds[2] = 0;
-//            dist_est_2 += 1;
-//        } 
-//        robot.forward(speed);
-//        if (check) {
-//            char buffer[5];
-//            sprintf(buffer,"%d", dist_est_1);
-//            robot.lcd_clear();
-//            robot.lcd_print(buffer,5);
-//        } else {
-//            char buffer2[5];
-//            sprintf(buffer2,"%d", dist_est_2);
-//            robot.lcd_clear();
-//            robot.lcd_print(buffer2,5);
-//        }
-//        
-//        if (button_enter.read() == 0) {
-//            check = not check;
-//        }  // wait for enter to be pressed
-//        
+        
         if (loop_check == true) {
             non_looped();
         } else {
@@ -196,24 +161,163 @@
 void looped()
 {
       // follow line until reaching a junction, determine its type and coordinates
-//    T_R.start();
-//    follow_line();
-//    
-//    if ( junction_detect() ) {
-//        time = T_R;
-//        T_R.stop();
-//        if (time > ) 
-//    }
-//
-//    
-//    simplify();
-//    
-//    robot.lcd_clear();
-//    robot.lcd_print(path,100);
+    if (t_restart){                // only start the timer if it isnt already started
+        t_coord.start();
+        t_restart = false;
+    }
+    
+    follow_line();
+    
+    if ( junction_detect() ) {
+                
+        int dist_est = t_coord.read();
+        t_coord.stop();
+        t_coord.reset();
+        
+        //auto dist_est = time - (time%1); // round the value (nearest second)
+        
+        if (dir == 'N'){ curr_coords[1] += dist_est; }  // y coord
+        if (dir == 'E'){ curr_coords[0] += dist_est; }  // x coord
+        if (dir == 'S'){ curr_coords[1] -= dist_est; }
+        if (dir == 'W'){ curr_coords[0] -= dist_est; }
+        
+        // check that the coordinates are not already in the list, if not add the point, if it is already return the point number and increment the explored
+        point[total_points] = total_points+1; // numbered 1 -> x
+        
+        coords_x[total_points] = curr_coords[0];
+        coords_y[total_points] = curr_coords[1];
+        
+        node_logic();
+        
+        // always try left first if there is a left. if node has already been explored once, then dont go left, go straight, if no straight then right
+        
+        total_points += 1;
+        // assign new node with new coordinates
+        t_restart = true; //restart the timer
+            
+    }
+    
+    // simplify();
+    
+    robot.lcd_clear();
+    robot.lcd_print(path,100);
 
     //robot.display_data();
 }
+
+void node_logic()
+{
+    bool north = false;
+    bool south = false;
+    bool east = false;
+    bool west = false;
+    
+    bool left = false;
+    bool straight = false;
+    bool right = false;
+    
+    int total = 1;   // starts at 1 because path entered on is counted
+    
+    if (sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) {
+        while ( (sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) && (sensor[1] > SENS_THRESH || sensor[2] > SENS_THRESH || sensor[3] > SENS_THRESH) ) {
+            robot.forward(speed);
+            robot.scan();
+            if ( sensor[0] > SENS_THRESH ) { left = true; }
+            if ( sensor[4] > SENS_THRESH ) { right = true; }
+        }
+        
+        if ( sensor[0] > SENS_THRESH && sensor[4] > SENS_THRESH && sensor[2] < SENS_THRESH ) {
+            wait(0.05);        // maybe change or replace w something better
+            robot.scan();
+        }
+        
+        robot.scan();
+        
+        if ( sensor[1] > SENS_THRESH || sensor[2] > SENS_THRESH || sensor[3] > SENS_THRESH ) {
+            straight = true;
+        }
+    }
+    
+    if (left) { total += 1; }
+    if (straight) { total += 1; }
+    if (right) { total += 1; }
+    
+    type[total_points] = total; 
+    
+    int angle = 0;
+    
+    if (dir == 'E') { angle = 90;}
+    else if (dir == 'S') { angle = 180;}
+    else if (dir == 'W') { angle = 270;}
+    
+    if (left) { 
+        angle += 270;
+        angle = angle % 360;
+        if (angle == 0) { north = true; }
+        if (angle == 180) { south = true; }
+        if (angle == 90) { east = true; }
+        if (angle == 270) { west = true; }
+        angle -= 270;
+        angle = angle % 360;
+    }
+    
+    if (right) { 
+        angle += 90;
+        angle = angle % 360;
+        if (angle == 0) { north = true; }
+        if (angle == 180) { south = true; }
+        if (angle == 90) { east = true; }
+        if (angle == 270) { west = true; }
+        angle -= 90;
+        angle = angle % 360;
+    }
       
+    if (straight) { 
+        if (angle == 0) { north = true; }
+        if (angle == 180) { south = true; }
+        if (angle == 90) { east = true; }
+        if (angle == 270) { west = true; }
+    }
+    
+    int turn_count = type[total_points] - explored[total_points];
+    
+    if (west == false) { turn_count += 1; }
+    if (north == false) { turn_count += 1; }
+    if (east == false) { turn_count += 1; }
+    if (south == false) { turn_count += 1; }
+    
+    turn_count = turn_count%4;
+    
+    char do_turn = turn_priority[turn_count];
+    
+    if (dir == 'E') { angle = 90;}
+    else if (dir == 'S') { angle = 180;}
+    else if (dir == 'W') { angle = 270;}
+    
+    unsigned int result = 0;
+    
+    if (do_turn == 'N') { dir = 'N'; result = 0; }
+    else if (do_turn == 'S') { dir = 'S'; result = 180; }
+    else if (do_turn == 'E') { dir = 'E'; result = 90; }
+    else if (do_turn == 'W') { dir = 'W'; result = 270; }
+    
+    result = result - angle;
+    
+    if (result == 0) { turn_select('S'); }
+    else if (result == 90) { turn_select('R'); }
+    else if (result == 180) { turn_select('B'); }
+    else if (result == 270) { turn_select('L'); }
+    // figure out a way to check what directions correlate to which turns based on direction currently facing.
+}
+
+void choose_turn() 
+{
+    // compares the type to the explored (type - explored), then uses that as an index for the struct of the turn order priority (turn_order[3] = {'E' , 'N', 'W', 'S'} 
+    // also takes into account current direction (if facing north, it wont take the south path for example)
+    // do this in point type
+    
+}
+
 void follow_line() 
 {
     robot.scan();