Anon Anon
/
MicroMousewithFloodFill
Updated with the Algorithm
Fork of MM_rat_Assignment4-newwest by
Diff: algorithm.h
- Revision:
- 8:22e399fe87a4
- Child:
- 9:97941581fe81
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/algorithm.h Tue Nov 28 21:45:07 2017 +0000 @@ -0,0 +1,143 @@ +#ifndef ALGO_H +#define ALGO_H +#include <cmath> +#include <stack> +#include <iostream> +#include <vector> +#include <algorithm> +#include "header.h" +struct geoCoord{ + geoCoord(int x_in, int y_in){x = x_in; y = y_in;} + geoCoord(){x = 0; y = 0;} + int x, y; + double distance; +}; + +bool compare(geoCoord* left, geoCoord* right){ + return left->distance > right->distance; +} + +int isWall(){ + float ir_r[4]; //ir_readings, in the order of l, r, lf, rf + FlashIRs(ir_r[0], ir_r[1], ir_r[2], ir_r[3]); + int return_val = 0; + if (ir_r[2] > 0.2f && ir_r[3] > 0.2f){ + return_val = return_val | 1; + } + if(ir_r[1] > 0.3f){ + return_val = return_val | 7; + } + if(ir_r[0] > 0.3f){ + return_val = return_val | 3; + } + return return_val; //This implementation is wrong but I got hungry. +} + +void goForward(int times){ + float l,r,lf,rf; + FlashIRs(l, r, lf, rf); + if(l < 0.17f && r < 0.17f){ + usePIDEncoder(); + } + else{ + usePIDBoth(); //Goes straight and uses PID using IR and the encoders + } + //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. + return; +} + +void MoveTo(geoCoord* current_coord, geoCoord* next, int curr_orientation){ //Note that turning automatically sets the global orientation constant + int diff_x = next->x - current_coord->x; + int diff_y = next->y - current_coord->y; + if(diff_x == 1){ + if(curr_orientation == SOUTH) + turn_left(1); + else if(curr_orientation == WEST){ + turn_right(2); + } + else if(curr_orientation == NORTH){ + turn_right(1); + } + } + else if(diff_x == -1){ + if(curr_orientation == SOUTH){ + turn_right(1); + } + else if(curr_orientation == NORTH){ + turn_left(1); + } + else if(curr_orientation == EAST){ + turn_right(2); + } + } + else if(diff_y == 1){ + if(curr_orientation == SOUTH){ + turn_right(2); + } + else if(curr_orientation == EAST){ + turn_left(1); + } + else if(curr_orientation == WEST){ + turn_right(1); + } + } + else if(diff_y == -1){ + if(curr_orientation == NORTH){ + turn_right(2); + } + else if(curr_orientation == EAST){ + turn_right(1); + } + else if(curr_orientation == WEST){ + turn_left(1); + } + } + else{ //We've made it to the end! + wait_ms(1000000000); + } + goForward(1); + return; +} + +geoCoord cellarray[16][16]; +geoCoord Target(8,8); //Target cell was assumed to be here. +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].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 + } + } + 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(); + int r_val = isWall(); + 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) + cells_to_traverse.push(&cellarray[current->x][current->y + 1]); + else if(i == 3 && current->x > 0) + cells_to_traverse.push(&cellarray[current->x - 1][current->y]); + else if(i == 7 && current->x < 15) + cells_to_traverse.push(&cellarray[current->x+1][current->y]); + else if(i == 5 && current->y > 0) + 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++){ + 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++){ + 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 + cells_to_traverse.pop(); + } +} +#endif \ No newline at end of file