Anon Anon / Mbed 2 deprecated MicroMousewithFloodFill

Updated with the Algorithm

Dependencies:   QEI mbed

Fork of MM_rat_Assignment4-newwest by Evan Brown

algorithm.h@9:97941581fe81, 2017-12-08 (annotated)

Committer:
Showboo
Date:
Fri Dec 08 05:14:27 2017 +0000
Revision:
9:97941581fe81
Parent:
8:22e399fe87a4 
Ready for Competition

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Showboo 8:22e399fe87a4 1 #ifndef ALGO_H
Showboo 8:22e399fe87a4 2 #define ALGO_H
Showboo 8:22e399fe87a4 3 #include <cmath>
Showboo 8:22e399fe87a4 4 #include <stack>
Showboo 8:22e399fe87a4 5 #include <iostream>
Showboo 8:22e399fe87a4 6 #include <vector>
Showboo 8:22e399fe87a4 7 #include <algorithm>
Showboo 8:22e399fe87a4 8 #include "header.h"
Showboo 8:22e399fe87a4 9 struct geoCoord{
Showboo 8:22e399fe87a4 10 geoCoord(int x_in, int y_in){x = x_in; y = y_in;}
Showboo 9:97941581fe81 11 geoCoord(){x = 0; y = 0; distance = 99999.0;}
Showboo 8:22e399fe87a4 12 int x, y;
Showboo 8:22e399fe87a4 13 double distance;
Showboo 8:22e399fe87a4 14 };
Showboo 8:22e399fe87a4 15
Showboo 8:22e399fe87a4 16 bool compare(geoCoord* left, geoCoord* right){
Showboo 8:22e399fe87a4 17 return left->distance > right->distance;
Showboo 8:22e399fe87a4 18 }
Showboo 8:22e399fe87a4 19
Showboo 8:22e399fe87a4 20 int isWall(){
Showboo 8:22e399fe87a4 21 float ir_r[4]; //ir_readings, in the order of l, r, lf, rf
Showboo 8:22e399fe87a4 22 FlashIRs(ir_r[0], ir_r[1], ir_r[2], ir_r[3]);
Showboo 8:22e399fe87a4 23 int return_val = 0;
Showboo 8:22e399fe87a4 24 if (ir_r[2] > 0.2f && ir_r[3] > 0.2f){
Showboo 8:22e399fe87a4 25 return_val = return_val | 1;
Showboo 8:22e399fe87a4 26 }
Showboo 8:22e399fe87a4 27 if(ir_r[1] > 0.3f){
Showboo 8:22e399fe87a4 28 return_val = return_val | 7;
Showboo 8:22e399fe87a4 29 }
Showboo 8:22e399fe87a4 30 if(ir_r[0] > 0.3f){
Showboo 8:22e399fe87a4 31 return_val = return_val | 3;
Showboo 8:22e399fe87a4 32 }
Showboo 8:22e399fe87a4 33 return return_val; //This implementation is wrong but I got hungry.
Showboo 8:22e399fe87a4 34 }
Showboo 8:22e399fe87a4 35
Showboo 8:22e399fe87a4 36 void goForward(int times){
Showboo 8:22e399fe87a4 37 float l,r,lf,rf;
Showboo 8:22e399fe87a4 38 FlashIRs(l, r, lf, rf);
Showboo 8:22e399fe87a4 39 if(l < 0.17f && r < 0.17f){
Showboo 9:97941581fe81 40 printf("goForward is using Encoder!");
Showboo 9:97941581fe81 41 RightEncoder.reset();
Showboo 9:97941581fe81 42 LeftEncoder.reset();
Showboo 8:22e399fe87a4 43 usePIDEncoder();
Showboo 9:97941581fe81 44 RightEncoder.reset();
Showboo 9:97941581fe81 45 LeftEncoder.reset();
Showboo 8:22e399fe87a4 46 }
Showboo 8:22e399fe87a4 47 else{
Showboo 9:97941581fe81 48 printf("goForward is using IR PID!");
Showboo 9:97941581fe81 49 RightEncoder.reset();
Showboo 9:97941581fe81 50 LeftEncoder.reset();
Showboo 8:22e399fe87a4 51 usePIDBoth(); //Goes straight and uses PID using IR and the encoders
Showboo 9:97941581fe81 52 RightEncoder.reset();
Showboo 9:97941581fe81 53 LeftEncoder.reset();
Showboo 8:22e399fe87a4 54 }
Showboo 8:22e399fe87a4 55 //Should be able to detect if there aren't walls and just work on the encoders
Showboo 8:22e399fe87a4 56 //And should also record the distance for just one cell.
Showboo 8:22e399fe87a4 57 return;
Showboo 8:22e399fe87a4 58 }
Showboo 8:22e399fe87a4 59
Showboo 8:22e399fe87a4 60 void MoveTo(geoCoord* current_coord, geoCoord* next, int curr_orientation){ //Note that turning automatically sets the global orientation constant
Showboo 9:97941581fe81 61 printf("Moving to: (%d, %d) From: (%d, %d): \n", current_coord->x, current_coord->y, next->x, next->y);
Showboo 8:22e399fe87a4 62 int diff_x = next->x - current_coord->x;
Showboo 8:22e399fe87a4 63 int diff_y = next->y - current_coord->y;
Showboo 8:22e399fe87a4 64 if(diff_x == 1){
Showboo 8:22e399fe87a4 65 if(curr_orientation == SOUTH)
Showboo 8:22e399fe87a4 66 turn_left(1);
Showboo 8:22e399fe87a4 67 else if(curr_orientation == WEST){
Showboo 8:22e399fe87a4 68 turn_right(2);
Showboo 8:22e399fe87a4 69 }
Showboo 8:22e399fe87a4 70 else if(curr_orientation == NORTH){
Showboo 8:22e399fe87a4 71 turn_right(1);
Showboo 8:22e399fe87a4 72 }
Showboo 8:22e399fe87a4 73 }
Showboo 8:22e399fe87a4 74 else if(diff_x == -1){
Showboo 8:22e399fe87a4 75 if(curr_orientation == SOUTH){
Showboo 8:22e399fe87a4 76 turn_right(1);
Showboo 8:22e399fe87a4 77 }
Showboo 8:22e399fe87a4 78 else if(curr_orientation == NORTH){
Showboo 8:22e399fe87a4 79 turn_left(1);
Showboo 8:22e399fe87a4 80 }
Showboo 8:22e399fe87a4 81 else if(curr_orientation == EAST){
Showboo 8:22e399fe87a4 82 turn_right(2);
Showboo 8:22e399fe87a4 83 }
Showboo 8:22e399fe87a4 84 }
Showboo 8:22e399fe87a4 85 else if(diff_y == 1){
Showboo 8:22e399fe87a4 86 if(curr_orientation == SOUTH){
Showboo 8:22e399fe87a4 87 turn_right(2);
Showboo 8:22e399fe87a4 88 }
Showboo 8:22e399fe87a4 89 else if(curr_orientation == EAST){
Showboo 8:22e399fe87a4 90 turn_left(1);
Showboo 8:22e399fe87a4 91 }
Showboo 8:22e399fe87a4 92 else if(curr_orientation == WEST){
Showboo 8:22e399fe87a4 93 turn_right(1);
Showboo 8:22e399fe87a4 94 }
Showboo 8:22e399fe87a4 95 }
Showboo 8:22e399fe87a4 96 else if(diff_y == -1){
Showboo 8:22e399fe87a4 97 if(curr_orientation == NORTH){
Showboo 8:22e399fe87a4 98 turn_right(2);
Showboo 8:22e399fe87a4 99 }
Showboo 8:22e399fe87a4 100 else if(curr_orientation == EAST){
Showboo 8:22e399fe87a4 101 turn_right(1);
Showboo 8:22e399fe87a4 102 }
Showboo 8:22e399fe87a4 103 else if(curr_orientation == WEST){
Showboo 8:22e399fe87a4 104 turn_left(1);
Showboo 8:22e399fe87a4 105 }
Showboo 8:22e399fe87a4 106 }
Showboo 8:22e399fe87a4 107 goForward(1);
Showboo 8:22e399fe87a4 108 return;
Showboo 8:22e399fe87a4 109 }
Showboo 8:22e399fe87a4 110
Showboo 8:22e399fe87a4 111 geoCoord cellarray[16][16];
Showboo 8:22e399fe87a4 112 geoCoord Target(8,8); //Target cell was assumed to be here.
Showboo 8:22e399fe87a4 113 geoCoord Start(0,0);
Showboo 8:22e399fe87a4 114
Showboo 8:22e399fe87a4 115 void algorithm(){ //Implementation of floodfill algorithm
Showboo 9:97941581fe81 116 for(int i = 0; i < 16; i++){
Showboo 9:97941581fe81 117 for(int k = 0; k < 16; k++){
Showboo 9:97941581fe81 118 cellarray[i][k].x = i; cellarray[i][k].y = k; //Initializes coordinates of geocoord array
Showboo 9:97941581fe81 119 }
Showboo 9:97941581fe81 120 }
Showboo 8:22e399fe87a4 121 for(int i = 0; i < 16; i++){
Showboo 8:22e399fe87a4 122 for(int k = 0; k < 16; k++){
Showboo 9:97941581fe81 123 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
Showboo 9:97941581fe81 124 printf("Cell Distance to (%d, %d): %.2f \n", i, k, cellarray[i][k].distance);
Showboo 8:22e399fe87a4 125 }
Showboo 8:22e399fe87a4 126 }
Showboo 8:22e399fe87a4 127 std::stack<geoCoord*> cells_to_traverse;
Showboo 8:22e399fe87a4 128 cells_to_traverse.push(&Start);
Showboo 8:22e399fe87a4 129 while(cells_to_traverse.size() > 0){
Showboo 8:22e399fe87a4 130 geoCoord* current = cells_to_traverse.top();
Showboo 9:97941581fe81 131 printf("CurrentGeoCoord (%d, %d): %.2f \n", current->x, current->y, current->distance);
Showboo 9:97941581fe81 132 cells_to_traverse.pop();//Theory: It's not pushing the correct cells.
Showboo 8:22e399fe87a4 133 int r_val = isWall();
Showboo 9:97941581fe81 134 printf("r_val: %d \n", r_val);
Showboo 8:22e399fe87a4 135 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
Showboo 8:22e399fe87a4 136 if(r_val&i == i){ //Potentially pushes 4 cells onto the stack
Showboo 8:22e399fe87a4 137 if(i == 1 && current->y < 15)
Showboo 8:22e399fe87a4 138 cells_to_traverse.push(&cellarray[current->x][current->y + 1]);
Showboo 8:22e399fe87a4 139 else if(i == 3 && current->x > 0)
Showboo 8:22e399fe87a4 140 cells_to_traverse.push(&cellarray[current->x - 1][current->y]);
Showboo 8:22e399fe87a4 141 else if(i == 7 && current->x < 15)
Showboo 8:22e399fe87a4 142 cells_to_traverse.push(&cellarray[current->x+1][current->y]);
Showboo 8:22e399fe87a4 143 else if(i == 5 && current->y > 0)
Showboo 8:22e399fe87a4 144 cells_to_traverse.push(&cellarray[current->x][current->y-1]); //Checks bounds for the potential cells to push.
Showboo 8:22e399fe87a4 145 }
Showboo 8:22e399fe87a4 146 std::vector<geoCoord*> neighboring_cells;
Showboo 9:97941581fe81 147 unsigned int t_size = cells_to_traverse.size();
Showboo 9:97941581fe81 148 for(int i = 0; i < t_size; i++){
Showboo 8:22e399fe87a4 149 neighboring_cells.push_back(cells_to_traverse.top()); //We temporarily put neighboring cells into a vector and sort them
Showboo 8:22e399fe87a4 150 cells_to_traverse.pop();
Showboo 8:22e399fe87a4 151 }
Showboo 8:22e399fe87a4 152 std::sort(neighboring_cells.begin(), neighboring_cells.end(), compare);
Showboo 9:97941581fe81 153 for(int i = 0; i < t_size; i++){
Showboo 8:22e399fe87a4 154 cells_to_traverse.push(neighboring_cells[i]); //Puts the sorted vector into reverse order back into the stack
Showboo 8:22e399fe87a4 155 }
Showboo 8:22e399fe87a4 156 MoveTo(current, cells_to_traverse.top(), global_orientation); //Moves to the cell that is the closest to the target cell
Showboo 8:22e399fe87a4 157 cells_to_traverse.pop();
Showboo 8:22e399fe87a4 158 }
Showboo 8:22e399fe87a4 159 }
Showboo 8:22e399fe87a4 160 #endif