Code to run the microcontrollers on the R5 competition bot
Dependencies: LineSensors mbed
r5map.cpp@17:5046b27f5441, 2015-04-18 (annotated)
- Committer:
- jmar11
- Date:
- Sat Apr 18 03:02:28 2015 +0000
- Revision:
- 17:5046b27f5441
- Child:
- 19:9f4510646c9e
Jaime's library
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jmar11 | 17:5046b27f5441 | 1 | #include "r5map.h" |
jmar11 | 17:5046b27f5441 | 2 | #include <iostream> |
jmar11 | 17:5046b27f5441 | 3 | #include <cstdlib> |
jmar11 | 17:5046b27f5441 | 4 | |
jmar11 | 17:5046b27f5441 | 5 | R5Map::R5Map(int conSize) : size(conSize) |
jmar11 | 17:5046b27f5441 | 6 | { |
jmar11 | 17:5046b27f5441 | 7 | int cellCount = 1; |
jmar11 | 17:5046b27f5441 | 8 | |
jmar11 | 17:5046b27f5441 | 9 | for(int i = 6; i >= 0; i--) |
jmar11 | 17:5046b27f5441 | 10 | { |
jmar11 | 17:5046b27f5441 | 11 | for(int j = 0; j < 7; j++) |
jmar11 | 17:5046b27f5441 | 12 | { |
jmar11 | 17:5046b27f5441 | 13 | map[j][i].number = cellCount++; |
jmar11 | 17:5046b27f5441 | 14 | } |
jmar11 | 17:5046b27f5441 | 15 | } |
jmar11 | 17:5046b27f5441 | 16 | |
jmar11 | 17:5046b27f5441 | 17 | switch(size) |
jmar11 | 17:5046b27f5441 | 18 | { |
jmar11 | 17:5046b27f5441 | 19 | case 5 : |
jmar11 | 17:5046b27f5441 | 20 | start.x = 5; |
jmar11 | 17:5046b27f5441 | 21 | start.y = 0; |
jmar11 | 17:5046b27f5441 | 22 | fin.x = 0; |
jmar11 | 17:5046b27f5441 | 23 | fin.y = 5; |
jmar11 | 17:5046b27f5441 | 24 | pos = start; |
jmar11 | 17:5046b27f5441 | 25 | break; |
jmar11 | 17:5046b27f5441 | 26 | case 6 : |
jmar11 | 17:5046b27f5441 | 27 | start.x = 5; |
jmar11 | 17:5046b27f5441 | 28 | start.y = 0; |
jmar11 | 17:5046b27f5441 | 29 | fin.x = 0; |
jmar11 | 17:5046b27f5441 | 30 | fin.y = 6; |
jmar11 | 17:5046b27f5441 | 31 | pos = start; |
jmar11 | 17:5046b27f5441 | 32 | break; |
jmar11 | 17:5046b27f5441 | 33 | case 7 : |
jmar11 | 17:5046b27f5441 | 34 | start.x = 6; |
jmar11 | 17:5046b27f5441 | 35 | start.y = 0; |
jmar11 | 17:5046b27f5441 | 36 | fin.x = 0; |
jmar11 | 17:5046b27f5441 | 37 | fin.y = 5; |
jmar11 | 17:5046b27f5441 | 38 | pos = start; |
jmar11 | 17:5046b27f5441 | 39 | break; |
jmar11 | 17:5046b27f5441 | 40 | default : |
jmar11 | 17:5046b27f5441 | 41 | cerr<<"ERROR: Invalid R5Map size. Exiting."<<endl; |
jmar11 | 17:5046b27f5441 | 42 | exit(EXIT_FAILURE); |
jmar11 | 17:5046b27f5441 | 43 | } |
jmar11 | 17:5046b27f5441 | 44 | } |
jmar11 | 17:5046b27f5441 | 45 | |
jmar11 | 17:5046b27f5441 | 46 | void R5Map::setPosition(position spot) |
jmar11 | 17:5046b27f5441 | 47 | { |
jmar11 | 17:5046b27f5441 | 48 | if(spot.x >= 0 && spot.y >= 0 && spot.x <= 6 && spot.y <= 6) |
jmar11 | 17:5046b27f5441 | 49 | { |
jmar11 | 17:5046b27f5441 | 50 | pos = spot; |
jmar11 | 17:5046b27f5441 | 51 | map[pos.x][pos.y].visited = true; |
jmar11 | 17:5046b27f5441 | 52 | } |
jmar11 | 17:5046b27f5441 | 53 | else |
jmar11 | 17:5046b27f5441 | 54 | { |
jmar11 | 17:5046b27f5441 | 55 | cerr<<"ERROR: Position assignment with R5Map::setPosition(position) outside map bounds!"<<endl; |
jmar11 | 17:5046b27f5441 | 56 | exit(EXIT_FAILURE); |
jmar11 | 17:5046b27f5441 | 57 | } |
jmar11 | 17:5046b27f5441 | 58 | } |
jmar11 | 17:5046b27f5441 | 59 | |
jmar11 | 17:5046b27f5441 | 60 | void R5Map::setStart(position newStart) |
jmar11 | 17:5046b27f5441 | 61 | { |
jmar11 | 17:5046b27f5441 | 62 | if(newStart.x >= 0 && newStart.y >= 0 && newStart.x <= 6 && newStart.y <= 6) |
jmar11 | 17:5046b27f5441 | 63 | start = newStart; |
jmar11 | 17:5046b27f5441 | 64 | else |
jmar11 | 17:5046b27f5441 | 65 | { |
jmar11 | 17:5046b27f5441 | 66 | cerr<<"ERROR: Start assignment with R5Map::setStart(position) outside map Bounds!"<<endl; |
jmar11 | 17:5046b27f5441 | 67 | exit(EXIT_FAILURE); |
jmar11 | 17:5046b27f5441 | 68 | } |
jmar11 | 17:5046b27f5441 | 69 | } |
jmar11 | 17:5046b27f5441 | 70 | |
jmar11 | 17:5046b27f5441 | 71 | void R5Map::setFinish(position newFin) |
jmar11 | 17:5046b27f5441 | 72 | { |
jmar11 | 17:5046b27f5441 | 73 | if(newFin.x>= 0 && newFin.y >= 0 && newFin.x <= 6 && newFin.y <= 6) |
jmar11 | 17:5046b27f5441 | 74 | fin = newFin; |
jmar11 | 17:5046b27f5441 | 75 | else |
jmar11 | 17:5046b27f5441 | 76 | { |
jmar11 | 17:5046b27f5441 | 77 | cerr<<"ERROR: Finsih assignment with R5Map::setFinish(position) outside map Bounds!"<<endl; |
jmar11 | 17:5046b27f5441 | 78 | exit(EXIT_FAILURE); |
jmar11 | 17:5046b27f5441 | 79 | } |
jmar11 | 17:5046b27f5441 | 80 | } |
jmar11 | 17:5046b27f5441 | 81 | |
jmar11 | 17:5046b27f5441 | 82 | void R5Map::setWall(position cell, direction dir, wallState state) |
jmar11 | 17:5046b27f5441 | 83 | { |
jmar11 | 17:5046b27f5441 | 84 | if(cell.x >= 0 && cell.y >= 0 && cell.x <= 6 && cell.y <= 6) |
jmar11 | 17:5046b27f5441 | 85 | { |
jmar11 | 17:5046b27f5441 | 86 | switch(dir) |
jmar11 | 17:5046b27f5441 | 87 | { |
jmar11 | 17:5046b27f5441 | 88 | case NORTH : map[cell.x][cell.y].north = state; |
jmar11 | 17:5046b27f5441 | 89 | break; |
jmar11 | 17:5046b27f5441 | 90 | case SOUTH : map[cell.x][cell.y].south = state; |
jmar11 | 17:5046b27f5441 | 91 | break; |
jmar11 | 17:5046b27f5441 | 92 | case EAST : map[cell.x][cell.y].east = state; |
jmar11 | 17:5046b27f5441 | 93 | break; |
jmar11 | 17:5046b27f5441 | 94 | case WEST : map[cell.x][cell.y].west = state; |
jmar11 | 17:5046b27f5441 | 95 | } |
jmar11 | 17:5046b27f5441 | 96 | } |
jmar11 | 17:5046b27f5441 | 97 | else |
jmar11 | 17:5046b27f5441 | 98 | { |
jmar11 | 17:5046b27f5441 | 99 | cerr<<"ERROR: Position selection in R5Map::setWall(position, cell, direction) outside map bounds!"<<endl; |
jmar11 | 17:5046b27f5441 | 100 | exit(EXIT_FAILURE); |
jmar11 | 17:5046b27f5441 | 101 | } |
jmar11 | 17:5046b27f5441 | 102 | } |
jmar11 | 17:5046b27f5441 | 103 | |
jmar11 | 17:5046b27f5441 | 104 | void R5Map::setCurWall(direction dir, wallState state) |
jmar11 | 17:5046b27f5441 | 105 | { |
jmar11 | 17:5046b27f5441 | 106 | switch(dir) |
jmar11 | 17:5046b27f5441 | 107 | { |
jmar11 | 17:5046b27f5441 | 108 | case NORTH : map[pos.x][pos.y].north = state; |
jmar11 | 17:5046b27f5441 | 109 | break; |
jmar11 | 17:5046b27f5441 | 110 | case SOUTH : map[pos.x][pos.y].south = state; |
jmar11 | 17:5046b27f5441 | 111 | break; |
jmar11 | 17:5046b27f5441 | 112 | case EAST : map[pos.x][pos.y].east = state; |
jmar11 | 17:5046b27f5441 | 113 | break; |
jmar11 | 17:5046b27f5441 | 114 | case WEST : map[pos.x][pos.y].west = state; |
jmar11 | 17:5046b27f5441 | 115 | } |
jmar11 | 17:5046b27f5441 | 116 | } |
jmar11 | 17:5046b27f5441 | 117 | |
jmar11 | 17:5046b27f5441 | 118 | void R5Map::setVisited(position cell) |
jmar11 | 17:5046b27f5441 | 119 | { |
jmar11 | 17:5046b27f5441 | 120 | if(cell.x >= 0 && cell.y >= 0 && cell.x <= 6 && cell.y <= 6) |
jmar11 | 17:5046b27f5441 | 121 | map[cell.x][cell.y].visited = true; |
jmar11 | 17:5046b27f5441 | 122 | else |
jmar11 | 17:5046b27f5441 | 123 | { |
jmar11 | 17:5046b27f5441 | 124 | cerr<<"ERROR: Position selection in R5Map::setVisited(position) outside of map bounds!"<<endl; |
jmar11 | 17:5046b27f5441 | 125 | exit(EXIT_FAILURE); |
jmar11 | 17:5046b27f5441 | 126 | } |
jmar11 | 17:5046b27f5441 | 127 | } |
jmar11 | 17:5046b27f5441 | 128 | |
jmar11 | 17:5046b27f5441 | 129 | void R5Map::setCurVisited() { map[pos.x][pos.y].visited = true; } |
jmar11 | 17:5046b27f5441 | 130 | |
jmar11 | 17:5046b27f5441 | 131 | position R5Map::getPosition() const { return pos; } |
jmar11 | 17:5046b27f5441 | 132 | |
jmar11 | 17:5046b27f5441 | 133 | position R5Map::getStart() const { return start; } |
jmar11 | 17:5046b27f5441 | 134 | |
jmar11 | 17:5046b27f5441 | 135 | position R5Map::getFinish() const { return fin; } |
jmar11 | 17:5046b27f5441 | 136 | |
jmar11 | 17:5046b27f5441 | 137 | wallState R5Map::getWall(position cell, direction dir) const |
jmar11 | 17:5046b27f5441 | 138 | { |
jmar11 | 17:5046b27f5441 | 139 | if(cell.x >= 0 && cell.y >= 0 && cell.x <= 6 && cell.y <= 6) |
jmar11 | 17:5046b27f5441 | 140 | { |
jmar11 | 17:5046b27f5441 | 141 | switch(dir) |
jmar11 | 17:5046b27f5441 | 142 | { |
jmar11 | 17:5046b27f5441 | 143 | case NORTH : return map[cell.x][cell.y].north; |
jmar11 | 17:5046b27f5441 | 144 | case SOUTH : return map[cell.x][cell.y].south; |
jmar11 | 17:5046b27f5441 | 145 | case EAST : return map[cell.x][cell.y].east; |
jmar11 | 17:5046b27f5441 | 146 | case WEST : return map[cell.x][cell.y].west; |
jmar11 | 17:5046b27f5441 | 147 | } |
jmar11 | 17:5046b27f5441 | 148 | } |
jmar11 | 17:5046b27f5441 | 149 | else |
jmar11 | 17:5046b27f5441 | 150 | { |
jmar11 | 17:5046b27f5441 | 151 | cerr<<"ERROR: Position selection in R5Map::getWall(position, direction) outside of map bounds!"<<endl; |
jmar11 | 17:5046b27f5441 | 152 | exit(EXIT_FAILURE); |
jmar11 | 17:5046b27f5441 | 153 | } |
jmar11 | 17:5046b27f5441 | 154 | } |
jmar11 | 17:5046b27f5441 | 155 | |
jmar11 | 17:5046b27f5441 | 156 | wallState R5Map::getCurWall(direction dir) const |
jmar11 | 17:5046b27f5441 | 157 | { |
jmar11 | 17:5046b27f5441 | 158 | switch(dir) |
jmar11 | 17:5046b27f5441 | 159 | { |
jmar11 | 17:5046b27f5441 | 160 | case NORTH : return map[pos.x][pos.y].north; |
jmar11 | 17:5046b27f5441 | 161 | case SOUTH : return map[pos.x][pos.y].south; |
jmar11 | 17:5046b27f5441 | 162 | case EAST : return map[pos.x][pos.y].east; |
jmar11 | 17:5046b27f5441 | 163 | case WEST : return map[pos.x][pos.y].west; |
jmar11 | 17:5046b27f5441 | 164 | } |
jmar11 | 17:5046b27f5441 | 165 | } |
jmar11 | 17:5046b27f5441 | 166 | |
jmar11 | 17:5046b27f5441 | 167 | bool R5Map::getVisited(position cell) const |
jmar11 | 17:5046b27f5441 | 168 | { |
jmar11 | 17:5046b27f5441 | 169 | if(cell.x >= 0 && cell.y >= 0 && cell.x <= 6 && cell.y <= 6) |
jmar11 | 17:5046b27f5441 | 170 | return map[cell.x][cell.y].visited; |
jmar11 | 17:5046b27f5441 | 171 | else |
jmar11 | 17:5046b27f5441 | 172 | { |
jmar11 | 17:5046b27f5441 | 173 | cerr<<"ERROR: Position selection in R5Map::getVisited(position) outside of map bounds!"<<endl; |
jmar11 | 17:5046b27f5441 | 174 | return false; |
jmar11 | 17:5046b27f5441 | 175 | } |
jmar11 | 17:5046b27f5441 | 176 | } |
jmar11 | 17:5046b27f5441 | 177 | |
jmar11 | 17:5046b27f5441 | 178 | bool R5Map::getCurVisited() const { return map[pos.x][pos.y].visited; } |
jmar11 | 17:5046b27f5441 | 179 | |
jmar11 | 17:5046b27f5441 | 180 | vector<char> R5Map::getSymbols(position cell) const |
jmar11 | 17:5046b27f5441 | 181 | { |
jmar11 | 17:5046b27f5441 | 182 | if(cell.x >= 0 && cell.y >= 0 && cell.x <= 6 && cell.y <= 6) |
jmar11 | 17:5046b27f5441 | 183 | return map[cell.x][cell.y].symbols; |
jmar11 | 17:5046b27f5441 | 184 | else |
jmar11 | 17:5046b27f5441 | 185 | { |
jmar11 | 17:5046b27f5441 | 186 | cerr<<"ERROR: Position selection in R5Map::getSymbols(position) outside of map bounds!"<<endl; |
jmar11 | 17:5046b27f5441 | 187 | vector<char> empty; |
jmar11 | 17:5046b27f5441 | 188 | return empty; |
jmar11 | 17:5046b27f5441 | 189 | } |
jmar11 | 17:5046b27f5441 | 190 | } |
jmar11 | 17:5046b27f5441 | 191 | |
jmar11 | 17:5046b27f5441 | 192 | vector<char> R5Map::getCurSymbols() const { return map[pos.x][pos.y].symbols; } |
jmar11 | 17:5046b27f5441 | 193 | |
jmar11 | 17:5046b27f5441 | 194 | int R5Map::getSize() const { return size; } |
jmar11 | 17:5046b27f5441 | 195 | |
jmar11 | 17:5046b27f5441 | 196 | void R5Map::printMap() const |
jmar11 | 17:5046b27f5441 | 197 | { |
jmar11 | 17:5046b27f5441 | 198 | for(int i = 0; i < size; i++) |
jmar11 | 17:5046b27f5441 | 199 | { |
jmar11 | 17:5046b27f5441 | 200 | for(int k = 0; k < size; k++) |
jmar11 | 17:5046b27f5441 | 201 | { |
jmar11 | 17:5046b27f5441 | 202 | cout<<"cell : "<<i+1<<','<<k+1<<endl; |
jmar11 | 17:5046b27f5441 | 203 | cout<<" North : "<<map[i][k].north<<endl; |
jmar11 | 17:5046b27f5441 | 204 | cout<<" South : "<<map[i][k].south<<endl; |
jmar11 | 17:5046b27f5441 | 205 | cout<<" East : "<<map[i][k].east<<endl; |
jmar11 | 17:5046b27f5441 | 206 | cout<<" West : "<<map[i][k].west<<endl; |
jmar11 | 17:5046b27f5441 | 207 | cout<<" Symbols : "; |
jmar11 | 17:5046b27f5441 | 208 | |
jmar11 | 17:5046b27f5441 | 209 | for(int j = 0; j < map[i][k].symbols.size(); j++) |
jmar11 | 17:5046b27f5441 | 210 | { |
jmar11 | 17:5046b27f5441 | 211 | cout<<map[i][k].symbols.at(j)<<" "; |
jmar11 | 17:5046b27f5441 | 212 | } |
jmar11 | 17:5046b27f5441 | 213 | cout<<endl<<"----------------------------"<<endl<<endl; |
jmar11 | 17:5046b27f5441 | 214 | } |
jmar11 | 17:5046b27f5441 | 215 | } |
jmar11 | 17:5046b27f5441 | 216 | } |
jmar11 | 17:5046b27f5441 | 217 | |
jmar11 | 17:5046b27f5441 | 218 | R5Map::~R5Map() { } |