![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Code to run the microcontrollers on the R5 competition bot
Dependencies: LineSensors mbed
navcontroller.cpp@19:9f4510646c9e, 2015-04-18 (annotated)
- Committer:
- Hypna
- Date:
- Sat Apr 18 03:50:32 2015 +0000
- Revision:
- 19:9f4510646c9e
- Parent:
- 17:5046b27f5441
- Child:
- 22:8f726dc175cd
Updating navcontroller and r5map
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jmar11 | 17:5046b27f5441 | 1 | #include <cstdlib> |
jmar11 | 17:5046b27f5441 | 2 | #include <cstring> |
jmar11 | 17:5046b27f5441 | 3 | #include <cstdio> |
jmar11 | 17:5046b27f5441 | 4 | #include <sstream> |
jmar11 | 17:5046b27f5441 | 5 | #include <string> |
jmar11 | 17:5046b27f5441 | 6 | #include <iostream> |
jmar11 | 17:5046b27f5441 | 7 | #include <vector> |
jmar11 | 17:5046b27f5441 | 8 | #include "navcontroller.h" |
jmar11 | 17:5046b27f5441 | 9 | |
jmar11 | 17:5046b27f5441 | 10 | NavController::NavController(int mapSize) : map(mapSize), orient(NORTH) |
jmar11 | 17:5046b27f5441 | 11 | { |
jmar11 | 17:5046b27f5441 | 12 | end.x = mapSize-1; |
jmar11 | 17:5046b27f5441 | 13 | end.y = mapSize-1; |
jmar11 | 17:5046b27f5441 | 14 | } |
jmar11 | 17:5046b27f5441 | 15 | |
jmar11 | 17:5046b27f5441 | 16 | void NavController::explore() |
jmar11 | 17:5046b27f5441 | 17 | { |
jmar11 | 17:5046b27f5441 | 18 | typename ::direction next; |
jmar11 | 17:5046b27f5441 | 19 | typename ::position newPos; |
jmar11 | 17:5046b27f5441 | 20 | |
jmar11 | 17:5046b27f5441 | 21 | while(map.getPosition() != end) |
jmar11 | 17:5046b27f5441 | 22 | { |
jmar11 | 17:5046b27f5441 | 23 | updateMap(); |
jmar11 | 17:5046b27f5441 | 24 | next = nextExploreMove(); |
jmar11 | 17:5046b27f5441 | 25 | sendMove(next); |
jmar11 | 17:5046b27f5441 | 26 | newPos = map.getPosition(); |
jmar11 | 17:5046b27f5441 | 27 | |
jmar11 | 17:5046b27f5441 | 28 | switch(next) |
jmar11 | 17:5046b27f5441 | 29 | { |
jmar11 | 17:5046b27f5441 | 30 | case NORTH : newPos.y++; |
jmar11 | 17:5046b27f5441 | 31 | break; |
jmar11 | 17:5046b27f5441 | 32 | case SOUTH : newPos.y--; |
jmar11 | 17:5046b27f5441 | 33 | break; |
jmar11 | 17:5046b27f5441 | 34 | case EAST : newPos.x++; |
jmar11 | 17:5046b27f5441 | 35 | break; |
jmar11 | 17:5046b27f5441 | 36 | case WEST : newPos.x--; |
jmar11 | 17:5046b27f5441 | 37 | } |
jmar11 | 17:5046b27f5441 | 38 | |
jmar11 | 17:5046b27f5441 | 39 | map.setPosition(newPos); |
jmar11 | 17:5046b27f5441 | 40 | } |
jmar11 | 17:5046b27f5441 | 41 | |
jmar11 | 17:5046b27f5441 | 42 | saveMap(); |
jmar11 | 17:5046b27f5441 | 43 | } |
jmar11 | 17:5046b27f5441 | 44 | |
Hypna | 19:9f4510646c9e | 45 | void NavController::prepReturn() |
jmar11 | 17:5046b27f5441 | 46 | { |
Hypna | 19:9f4510646c9e | 47 | stack<R5Map::direction> temp; |
jmar11 | 17:5046b27f5441 | 48 | |
jmar11 | 17:5046b27f5441 | 49 | while(!path.empty()) |
jmar11 | 17:5046b27f5441 | 50 | { |
jmar11 | 17:5046b27f5441 | 51 | temp.push(path.top()); |
jmar11 | 17:5046b27f5441 | 52 | path.pop(); |
jmar11 | 17:5046b27f5441 | 53 | } |
jmar11 | 17:5046b27f5441 | 54 | |
jmar11 | 17:5046b27f5441 | 55 | path = temp; |
jmar11 | 17:5046b27f5441 | 56 | } |
jmar11 | 17:5046b27f5441 | 57 | |
Hypna | 19:9f4510646c9e | 58 | R5Map::direction NavController::nextTraceMove() |
jmar11 | 17:5046b27f5441 | 59 | { |
Hypna | 19:9f4510646c9e | 60 | R5Map::position newPos = map.getPosition(); |
Hypna | 19:9f4510646c9e | 61 | R5Map::nextMove; |
Hypna | 19:9f4510646c9e | 62 | |
Hypna | 19:9f4510646c9e | 63 | nextMove = path.top(); |
Hypna | 19:9f4510646c9e | 64 | path.pop(); |
Hypna | 19:9f4510646c9e | 65 | |
Hypna | 19:9f4510646c9e | 66 | switch(nextMove) |
Hypna | 19:9f4510646c9e | 67 | { |
Hypna | 19:9f4510646c9e | 68 | case NORTH : newPos.y++; |
Hypna | 19:9f4510646c9e | 69 | break; |
Hypna | 19:9f4510646c9e | 70 | case SOUTH : newPos.y--; |
Hypna | 19:9f4510646c9e | 71 | break; |
Hypna | 19:9f4510646c9e | 72 | case EAST : newPos.x++; |
Hypna | 19:9f4510646c9e | 73 | break; |
Hypna | 19:9f4510646c9e | 74 | case WEST : newPos.x--; |
Hypna | 19:9f4510646c9e | 75 | } |
Hypna | 19:9f4510646c9e | 76 | |
Hypna | 19:9f4510646c9e | 77 | map.setPosition(newPos); |
Hypna | 19:9f4510646c9e | 78 | return nextMove; |
Hypna | 19:9f4510646c9e | 79 | } |
Hypna | 19:9f4510646c9e | 80 | |
Hypna | 19:9f4510646c9e | 81 | R5Map::direction NavController::nextExploreMove() |
Hypna | 19:9f4510646c9e | 82 | { |
Hypna | 19:9f4510646c9e | 83 | R5Map::position current = map.getPosition(); |
Hypna | 19:9f4510646c9e | 84 | R5Map::position dest = map.getFinish(); |
jmar11 | 17:5046b27f5441 | 85 | |
jmar11 | 17:5046b27f5441 | 86 | int northWgt = dest.y - current.y; |
jmar11 | 17:5046b27f5441 | 87 | int southWgt = current.y; |
jmar11 | 17:5046b27f5441 | 88 | int eastWgt = current.x; |
jmar11 | 17:5046b27f5441 | 89 | int westWgt = current.x - dest.x; |
jmar11 | 17:5046b27f5441 | 90 | |
Hypna | 19:9f4510646c9e | 91 | R5Map::direction nextMove; |
jmar11 | 17:5046b27f5441 | 92 | |
jmar11 | 17:5046b27f5441 | 93 | if(map.getVisited(NORTH) || map.getCurWall(NORTH) == CLOSED) |
jmar11 | 17:5046b27f5441 | 94 | northWgt = 0; |
jmar11 | 17:5046b27f5441 | 95 | |
jmar11 | 17:5046b27f5441 | 96 | if(map.getVisited(SOUTH) || map.getCurWall(SOUTH) == CLOSED) |
jmar11 | 17:5046b27f5441 | 97 | southWgt = 0; |
jmar11 | 17:5046b27f5441 | 98 | |
jmar11 | 17:5046b27f5441 | 99 | if(map.getVisited(EAST) || map.getCurWall(EAST) == CLOSED) |
jmar11 | 17:5046b27f5441 | 100 | eastWgt = 0; |
jmar11 | 17:5046b27f5441 | 101 | |
jmar11 | 17:5046b27f5441 | 102 | if(map.getVisited(WEST) || map.getCurWall(WEST) == CLOSED) |
jmar11 | 17:5046b27f5441 | 103 | westWgt = 0; |
jmar11 | 17:5046b27f5441 | 104 | |
jmar11 | 17:5046b27f5441 | 105 | if(northWgt != 0 || westWgt != 0) |
jmar11 | 17:5046b27f5441 | 106 | { |
Hypna | 19:9f4510646c9e | 107 | if(northWgt > westWgt) |
jmar11 | 17:5046b27f5441 | 108 | nextMove = NORTH; |
jmar11 | 17:5046b27f5441 | 109 | else |
jmar11 | 17:5046b27f5441 | 110 | nextMove = WEST; |
jmar11 | 17:5046b27f5441 | 111 | |
jmar11 | 17:5046b27f5441 | 112 | path.push(nextMove); |
jmar11 | 17:5046b27f5441 | 113 | } |
jmar11 | 17:5046b27f5441 | 114 | else if(southWgt != 0 || eastWgt != 0) |
jmar11 | 17:5046b27f5441 | 115 | { |
jmar11 | 17:5046b27f5441 | 116 | if(southWgt > eastWgt) |
jmar11 | 17:5046b27f5441 | 117 | nextMove = SOUTH; |
jmar11 | 17:5046b27f5441 | 118 | else |
jmar11 | 17:5046b27f5441 | 119 | nextMove = EAST; |
jmar11 | 17:5046b27f5441 | 120 | |
jmar11 | 17:5046b27f5441 | 121 | path.push(nextMove); |
jmar11 | 17:5046b27f5441 | 122 | } |
jmar11 | 17:5046b27f5441 | 123 | else |
jmar11 | 17:5046b27f5441 | 124 | { |
Hypna | 19:9f4510646c9e | 125 | R5Map::direction lastMove = path.top(); |
jmar11 | 17:5046b27f5441 | 126 | path.pop(); |
jmar11 | 17:5046b27f5441 | 127 | |
jmar11 | 17:5046b27f5441 | 128 | switch(lastMove) |
jmar11 | 17:5046b27f5441 | 129 | { |
jmar11 | 17:5046b27f5441 | 130 | case NORTH : nextMove = SOUTH; |
jmar11 | 17:5046b27f5441 | 131 | break; |
jmar11 | 17:5046b27f5441 | 132 | case SOUTH : nextMove = NORTH; |
jmar11 | 17:5046b27f5441 | 133 | break; |
jmar11 | 17:5046b27f5441 | 134 | case EAST : nextMove = WEST; |
jmar11 | 17:5046b27f5441 | 135 | break; |
jmar11 | 17:5046b27f5441 | 136 | case WEST : nextMove = EAST; |
jmar11 | 17:5046b27f5441 | 137 | } |
jmar11 | 17:5046b27f5441 | 138 | } |
jmar11 | 17:5046b27f5441 | 139 | |
jmar11 | 17:5046b27f5441 | 140 | return nextMove; |
jmar11 | 17:5046b27f5441 | 141 | } |
jmar11 | 17:5046b27f5441 | 142 | |
jmar11 | 17:5046b27f5441 | 143 | void NavController::updateMap() |
jmar11 | 17:5046b27f5441 | 144 | { |
jmar11 | 17:5046b27f5441 | 145 | if(!map.getCurVisited()) |
jmar11 | 17:5046b27f5441 | 146 | { |
jmar11 | 17:5046b27f5441 | 147 | readSensor(NORTH) ? map.setCurWall(NORTH, CLOSED) : map.setCurWall(NORTH, OPEN); |
jmar11 | 17:5046b27f5441 | 148 | readSensor(SOUTH) ? map.setCurWall(SOUTH, CLOSED) : map.setCurWall(SOUTH, OPEN); |
jmar11 | 17:5046b27f5441 | 149 | readSensor(EAST) ? map.setCurWall(EAST, CLOSED) : map.setCurWall(EAST, OPEN); |
jmar11 | 17:5046b27f5441 | 150 | readSensor(WEST) ? map.setCurWall(WEST, CLOSED) : map.setCurWall(WEST, OPEN); |
jmar11 | 17:5046b27f5441 | 151 | } |
jmar11 | 17:5046b27f5441 | 152 | } |
jmar11 | 17:5046b27f5441 | 153 | |
Hypna | 19:9f4510646c9e | 154 | bool NavController::readSensor(R5Map::direction wall) |
jmar11 | 17:5046b27f5441 | 155 | { |
Hypna | 19:9f4510646c9e | 156 | int diff = wall - orient; |
jmar11 | 17:5046b27f5441 | 157 | |
Hypna | 19:9f4510646c9e | 158 | switch(diff) |
jmar11 | 17:5046b27f5441 | 159 | { |
jmar11 | 17:5046b27f5441 | 160 | case 0: |
jmar11 | 17:5046b27f5441 | 161 | digitalRead(SENS_FORWARD); |
jmar11 | 17:5046b27f5441 | 162 | case 1: |
jmar11 | 17:5046b27f5441 | 163 | digitalRead(SENS_RIGHT); |
jmar11 | 17:5046b27f5441 | 164 | case 2: |
jmar11 | 17:5046b27f5441 | 165 | digitalRead(SENS_BACK); |
jmar11 | 17:5046b27f5441 | 166 | case 3: |
jmar11 | 17:5046b27f5441 | 167 | digitalRead(SENS_LEFT); |
jmar11 | 17:5046b27f5441 | 168 | } |
Hypna | 19:9f4510646c9e | 169 | } |