Texas State IEEE / Mbed 2 deprecated R5

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?

UserRevisionLine numberNew 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 }