Revision 31:f7a8a9b82bc1, committed 2017-05-28

Comitter:

kolanery

Date:

Sun May 28 09:54:40 2017 +0000

Parent:

30:daf286ac049f

Commit message:

Update

Changed in this revision

--- a/Control/drivecontrol.cpp	Sun May 28 06:49:41 2017 +0000
+++ b/Control/drivecontrol.cpp	Sun May 28 09:54:40 2017 +0000
@@ -26,6 +26,12 @@
 float left_speed, right_speed, motor_speed;
 float MOTOR_BASE_SPEED = 20.0f; //12.0f;
 float cool_down_offset = 0.0f;
+//
+//    bool right_front_wall = (rightFrontIR.readIR() * 1000) > 355.0f;//350.39f;
+//    bool left_front_wall = (leftFrontIR.readIR() * 1000) > 315.0f;//310.25f;
+
+float RIGHT_FRONT_THRES = 355.0f;
+float LEFT_FRONT_THRES = 315.0f;
 
 bool enter_right_wall_pid_debug = false;
 
@@ -165,13 +171,19 @@
     pid_controller::turn(false);
 }
 
+void DriveControl::set_wall_follower_sensor_thres(){
+    //TODO
+    RIGHT_FRONT_THRES = 300.0f;
+    LEFT_FRONT_THRES = 250.0f;
+}
+
 void DriveControl::turn_right() {
     // TODO: Add PID Control
     pid_controller::turn(true);
 }
 
 void DriveControl::set_wall_follower_speed() {
-    MOTOR_BASE_SPEED  = 30;
+    MOTOR_BASE_SPEED  = 25;
 }
 
 void DriveControl::turn_around() {
@@ -222,8 +234,10 @@
 }
 
 bool DriveControl::has_front_wall() {
-    bool right_front_wall = (rightFrontIR.readIR() * 1000) > 355.0f;//350.39f;
-    bool left_front_wall = (leftFrontIR.readIR() * 1000) > 315.0f;//310.25f;
+//    float RIGHT_FRONT_THRES = 355.0f;
+//float LEFT_FRONT_THRES = 315.0f;
+    bool right_front_wall = (rightFrontIR.readIR() * 1000) > RIGHT_FRONT_THRES;//350.39f;
+    bool left_front_wall = (leftFrontIR.readIR() * 1000) > LEFT_FRONT_THRES;//310.25f;
     return right_front_wall || left_front_wall;
 }
 

--- a/Control/drivecontrol.h	Sun May 28 06:49:41 2017 +0000
+++ b/Control/drivecontrol.h	Sun May 28 09:54:40 2017 +0000
@@ -49,6 +49,8 @@
     
     void set_wall_follower_speed();
     
+    void set_wall_follower_sensor_thres();
+    
     bool should_finish_turn_right();
     
     bool right_wall_pid_debug();

--- a/Maze/maze_solver.cpp	Sun May 28 06:49:41 2017 +0000
+++ b/Maze/maze_solver.cpp	Sun May 28 09:54:40 2017 +0000
@@ -285,7 +285,6 @@
             }
         }
         if (open_neighbor.empty()) {
-            printf("This should never happen, error\n");
             neighbor.clear();
             continue;
         }

--- a/Maze/maze_solver.h	Sun May 28 06:49:41 2017 +0000
+++ b/Maze/maze_solver.h	Sun May 28 09:54:40 2017 +0000
@@ -100,6 +100,8 @@
     void clear_stack();
     
     bool can_reset_mouse;
+    
+    bool is_center;
 
     void move_one_cell();
     

--- a/main.cpp	Sun May 28 06:49:41 2017 +0000
+++ b/main.cpp	Sun May 28 09:54:40 2017 +0000
@@ -70,7 +70,23 @@
 //         //my_mouse = new Mouse(driver);
 //       }
        if (finished_traverse_one_cell == true) {
-            count_move++;
+//            if (!driver->has_right_wall() && !driver->has_left_wall()){
+//                flash_led(0,0,0,0);    
+//            }
+//            else if (!driver->has_left_wall()){
+//                flash_led(1,0,0,0);
+//            }
+//            else if (!driver->has_right_wall()){
+//                flash_led(0,1,1,1);
+//            }
+//            else{
+//                flash_led(0,0,0,0);
+//            }
+
+            if (my_mouse->center_found()) {
+                flash_led(1,1,1,1);
+            }
+            //count_move++;
 //            if (count_move > 18 && my_mouse->can_reset_mouse) {
 //                my_mouse->clear_stack();
 //                delete my_mouse;
@@ -79,33 +95,32 @@
 //                wait(2);
 //                run_flood_fill_algorithm();
 //            }
-            if (count_move > 40) {
-                my_mouse->clear_stack();
-                delete my_mouse;
-                count_move = 0;
-                flash_led(0,1,1,1);
-                wait(2);
-            }
+//            if (count_move > 30) {
+//                my_mouse->clear_stack();
+//                count_move = 0;
+//                flash_led(0,1,1,1);
+//                wait(2);
+//            }
             state = my_mouse->solve_maze();
             wait(0.5);
             if(state == RIGHT){
-                flash_led(0,0,1,1);
+                //flash_led(0,0,1,1);
                 wait(0.25);
                 driver->resetEncoders();
             }
             else if(state == LEFT){
-                flash_led(1,1,0,0);
+                //flash_led(1,1,0,0);
                 wait(0.25);
                 driver->resetEncoders();
             }
             else if(state == UTURN){
-                flash_led(1,1,1,0);
+                //flash_led(1,1,1,0);
                 wait(0.25);
                 driver->resetEncoders();
                 count = 0;
             }else{
                 state = STRAIGHT;
-                flash_led(1, 0, 0, 0);
+                //flash_led(1, 0, 0, 0);
             }
             
             finished_traverse_one_cell = false;
@@ -172,6 +187,7 @@
 void run_heuristic_wall_follower_algorithm() {
     DriveControl * driver = new DriveControl (START_POS, END_POS);
     driver->set_wall_follower_speed();
+    driver->set_wall_follower_sensor_thres();
     driver->resetEncoders();
     int state, count = 0;
     int heuristic = 0;