Diff: algorithm.h

Revision:

9:97941581fe81

Parent:

8:22e399fe87a4

--- a/algorithm.h	Tue Nov 28 21:45:07 2017 +0000
+++ b/algorithm.h	Fri Dec 08 05:14:27 2017 +0000
@@ -8,7 +8,7 @@
 #include "header.h"
 struct geoCoord{
     geoCoord(int x_in, int y_in){x = x_in; y = y_in;}
-    geoCoord(){x = 0; y = 0;}
+    geoCoord(){x = 0; y = 0; distance = 99999.0;}
     int x, y;
     double distance;
 };
@@ -37,10 +37,20 @@
     float l,r,lf,rf;
     FlashIRs(l, r, lf, rf);
     if(l < 0.17f && r < 0.17f){
+         printf("goForward is using Encoder!");
+        RightEncoder.reset();
+        LeftEncoder.reset();
         usePIDEncoder();    
+        RightEncoder.reset();
+        LeftEncoder.reset();
     }
     else{
+        printf("goForward is using IR PID!");
+        RightEncoder.reset();
+        LeftEncoder.reset();
         usePIDBoth(); //Goes straight and uses PID using IR and the encoders
+        RightEncoder.reset();
+        LeftEncoder.reset();
     }
     //Should be able to detect if there aren't walls and just work on the encoders
     //And should also record the distance for just one cell.
@@ -48,6 +58,7 @@
 }
 
 void MoveTo(geoCoord* current_coord, geoCoord* next, int curr_orientation){ //Note that turning automatically sets the global orientation constant
+    printf("Moving to: (%d, %d) From: (%d, %d):  \n", current_coord->x, current_coord->y, next->x, next->y);
     int diff_x = next->x - current_coord->x;
     int diff_y = next->y - current_coord->y;
     if(diff_x == 1){
@@ -93,9 +104,6 @@
             turn_left(1);
         }
     }
-    else{ //We've made it to the end!
-        wait_ms(1000000000);
-    }
     goForward(1);
     return;
 }
@@ -105,17 +113,25 @@
 geoCoord Start(0,0);
 
 void algorithm(){ //Implementation of floodfill algorithm
+for(int i = 0; i < 16; i++){
+        for(int k = 0; k < 16; k++){
+         cellarray[i][k].x = i; cellarray[i][k].y = k; //Initializes coordinates of geocoord array
+        }
+    }
     for(int i = 0; i < 16; i++){
         for(int k = 0; k < 16; k++){
-            cellarray[i][k].distance = std::sqrt((double)((double)cellarray[i][k].x - (double)Start.x)*((double)cellarray[i][k].x - (double)Start.x) + ((double)cellarray[i][k].y - (double)Start.y)*((double)cellarray[i][k].y - (double)Start.y)); //Initializes the distances of the cellarray
+            cellarray[i][k].distance = std::sqrt((double)(cellarray[i][k].x - Start.x)*(double)(cellarray[i][k].x - Start.x) + (double)(cellarray[i][k].y - Start.y)*(double)(cellarray[i][k].y - Start.y)); //Initializes the distances of the cellarray
+            printf("Cell Distance to (%d, %d): %.2f \n", i, k, cellarray[i][k].distance);
         }
     }
     std::stack<geoCoord*> cells_to_traverse;
     cells_to_traverse.push(&Start);
     while(cells_to_traverse.size() > 0){
         geoCoord* current = cells_to_traverse.top();
-        cells_to_traverse.pop();
+        printf("CurrentGeoCoord (%d, %d): %.2f \n", current->x, current->y, current->distance);
+        cells_to_traverse.pop();//Theory: It's not pushing the correct cells.
         int r_val = isWall();
+        printf("r_val: %d \n", r_val);
         for(int i = 1; i < 8; i+= 2) //1 for Forward, 3 for Left, 5 for South, 7 for Right. South probably isn't going to be a thing
         if(r_val&i == i){ //Potentially pushes 4 cells onto the stack
             if(i == 1 && current->y < 15)
@@ -128,12 +144,13 @@
                 cells_to_traverse.push(&cellarray[current->x][current->y-1]); //Checks bounds for the potential cells to push.
         }
         std::vector<geoCoord*> neighboring_cells;
-        for(int i = 0; i < cells_to_traverse.size(); i++){
+        unsigned int t_size = cells_to_traverse.size();
+        for(int i = 0; i < t_size; i++){
             neighboring_cells.push_back(cells_to_traverse.top()); //We temporarily put neighboring cells into a vector and sort them
             cells_to_traverse.pop();
         }
         std::sort(neighboring_cells.begin(), neighboring_cells.end(), compare);
-        for(int i = 0; i < neighboring_cells.size(); i++){
+        for(int i = 0; i < t_size; i++){
             cells_to_traverse.push(neighboring_cells[i]); //Puts the sorted vector into reverse order back into the stack
         }
         MoveTo(current, cells_to_traverse.top(), global_orientation); //Moves to the cell that is the closest to the target cell