Anon Anon / Mbed 2 deprecated MicroMousewithFloodFill

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   QEI mbed

Fork of MM_rat_Assignment4-newwest by Evan Brown

algorithm.h@8:22e399fe87a4, 2017-11-28 (annotated)

Committer:
Showboo
Date:
Tue Nov 28 21:45:07 2017 +0000
Revision:
8:22e399fe87a4
Child:
9:97941581fe81
Updated for the algorithm

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 8:22e399fe87a4 11 geoCoord(){x = 0; y = 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 8:22e399fe87a4 40 usePIDEncoder();
Showboo 8:22e399fe87a4 41 }
Showboo 8:22e399fe87a4 42 else{
Showboo 8:22e399fe87a4 43 usePIDBoth(); //Goes straight and uses PID using IR and the encoders
Showboo 8:22e399fe87a4 44 }
Showboo 8:22e399fe87a4 45 //Should be able to detect if there aren't walls and just work on the encoders
Showboo 8:22e399fe87a4 46 //And should also record the distance for just one cell.
Showboo 8:22e399fe87a4 47 return;
Showboo 8:22e399fe87a4 48 }
Showboo 8:22e399fe87a4 49
Showboo 8:22e399fe87a4 50 void MoveTo(geoCoord* current_coord, geoCoord* next, int curr_orientation){ //Note that turning automatically sets the global orientation constant
Showboo 8:22e399fe87a4 51 int diff_x = next->x - current_coord->x;
Showboo 8:22e399fe87a4 52 int diff_y = next->y - current_coord->y;
Showboo 8:22e399fe87a4 53 if(diff_x == 1){
Showboo 8:22e399fe87a4 54 if(curr_orientation == SOUTH)
Showboo 8:22e399fe87a4 55 turn_left(1);
Showboo 8:22e399fe87a4 56 else if(curr_orientation == WEST){
Showboo 8:22e399fe87a4 57 turn_right(2);
Showboo 8:22e399fe87a4 58 }
Showboo 8:22e399fe87a4 59 else if(curr_orientation == NORTH){
Showboo 8:22e399fe87a4 60 turn_right(1);
Showboo 8:22e399fe87a4 61 }
Showboo 8:22e399fe87a4 62 }
Showboo 8:22e399fe87a4 63 else if(diff_x == -1){
Showboo 8:22e399fe87a4 64 if(curr_orientation == SOUTH){
Showboo 8:22e399fe87a4 65 turn_right(1);
Showboo 8:22e399fe87a4 66 }
Showboo 8:22e399fe87a4 67 else if(curr_orientation == NORTH){
Showboo 8:22e399fe87a4 68 turn_left(1);
Showboo 8:22e399fe87a4 69 }
Showboo 8:22e399fe87a4 70 else if(curr_orientation == EAST){
Showboo 8:22e399fe87a4 71 turn_right(2);
Showboo 8:22e399fe87a4 72 }
Showboo 8:22e399fe87a4 73 }
Showboo 8:22e399fe87a4 74 else if(diff_y == 1){
Showboo 8:22e399fe87a4 75 if(curr_orientation == SOUTH){
Showboo 8:22e399fe87a4 76 turn_right(2);
Showboo 8:22e399fe87a4 77 }
Showboo 8:22e399fe87a4 78 else if(curr_orientation == EAST){
Showboo 8:22e399fe87a4 79 turn_left(1);
Showboo 8:22e399fe87a4 80 }
Showboo 8:22e399fe87a4 81 else if(curr_orientation == WEST){
Showboo 8:22e399fe87a4 82 turn_right(1);
Showboo 8:22e399fe87a4 83 }
Showboo 8:22e399fe87a4 84 }
Showboo 8:22e399fe87a4 85 else if(diff_y == -1){
Showboo 8:22e399fe87a4 86 if(curr_orientation == NORTH){
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_right(1);
Showboo 8:22e399fe87a4 91 }
Showboo 8:22e399fe87a4 92 else if(curr_orientation == WEST){
Showboo 8:22e399fe87a4 93 turn_left(1);
Showboo 8:22e399fe87a4 94 }
Showboo 8:22e399fe87a4 95 }
Showboo 8:22e399fe87a4 96 else{ //We've made it to the end!
Showboo 8:22e399fe87a4 97 wait_ms(1000000000);
Showboo 8:22e399fe87a4 98 }
Showboo 8:22e399fe87a4 99 goForward(1);
Showboo 8:22e399fe87a4 100 return;
Showboo 8:22e399fe87a4 101 }
Showboo 8:22e399fe87a4 102
Showboo 8:22e399fe87a4 103 geoCoord cellarray[16][16];
Showboo 8:22e399fe87a4 104 geoCoord Target(8,8); //Target cell was assumed to be here.
Showboo 8:22e399fe87a4 105 geoCoord Start(0,0);
Showboo 8:22e399fe87a4 106
Showboo 8:22e399fe87a4 107 void algorithm(){ //Implementation of floodfill algorithm
Showboo 8:22e399fe87a4 108 for(int i = 0; i < 16; i++){
Showboo 8:22e399fe87a4 109 for(int k = 0; k < 16; k++){
Showboo 8:22e399fe87a4 110 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
Showboo 8:22e399fe87a4 111 }
Showboo 8:22e399fe87a4 112 }
Showboo 8:22e399fe87a4 113 std::stack<geoCoord*> cells_to_traverse;
Showboo 8:22e399fe87a4 114 cells_to_traverse.push(&Start);
Showboo 8:22e399fe87a4 115 while(cells_to_traverse.size() > 0){
Showboo 8:22e399fe87a4 116 geoCoord* current = cells_to_traverse.top();
Showboo 8:22e399fe87a4 117 cells_to_traverse.pop();
Showboo 8:22e399fe87a4 118 int r_val = isWall();
Showboo 8:22e399fe87a4 119 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 120 if(r_val&i == i){ //Potentially pushes 4 cells onto the stack
Showboo 8:22e399fe87a4 121 if(i == 1 && current->y < 15)
Showboo 8:22e399fe87a4 122 cells_to_traverse.push(&cellarray[current->x][current->y + 1]);
Showboo 8:22e399fe87a4 123 else if(i == 3 && current->x > 0)
Showboo 8:22e399fe87a4 124 cells_to_traverse.push(&cellarray[current->x - 1][current->y]);
Showboo 8:22e399fe87a4 125 else if(i == 7 && current->x < 15)
Showboo 8:22e399fe87a4 126 cells_to_traverse.push(&cellarray[current->x+1][current->y]);
Showboo 8:22e399fe87a4 127 else if(i == 5 && current->y > 0)
Showboo 8:22e399fe87a4 128 cells_to_traverse.push(&cellarray[current->x][current->y-1]); //Checks bounds for the potential cells to push.
Showboo 8:22e399fe87a4 129 }
Showboo 8:22e399fe87a4 130 std::vector<geoCoord*> neighboring_cells;
Showboo 8:22e399fe87a4 131 for(int i = 0; i < cells_to_traverse.size(); i++){
Showboo 8:22e399fe87a4 132 neighboring_cells.push_back(cells_to_traverse.top()); //We temporarily put neighboring cells into a vector and sort them
Showboo 8:22e399fe87a4 133 cells_to_traverse.pop();
Showboo 8:22e399fe87a4 134 }
Showboo 8:22e399fe87a4 135 std::sort(neighboring_cells.begin(), neighboring_cells.end(), compare);
Showboo 8:22e399fe87a4 136 for(int i = 0; i < neighboring_cells.size(); i++){
Showboo 8:22e399fe87a4 137 cells_to_traverse.push(neighboring_cells[i]); //Puts the sorted vector into reverse order back into the stack
Showboo 8:22e399fe87a4 138 }
Showboo 8:22e399fe87a4 139 MoveTo(current, cells_to_traverse.top(), global_orientation); //Moves to the cell that is the closest to the target cell
Showboo 8:22e399fe87a4 140 cells_to_traverse.pop();
Showboo 8:22e399fe87a4 141 }
Showboo 8:22e399fe87a4 142 }
Showboo 8:22e399fe87a4 143 #endif