Texas State IEEE / Mbed 2 deprecated R5

Code to run the microcontrollers on the R5 competition bot

Dependencies:   LineSensors mbed

navcontroller.cpp@22:8f726dc175cd, 2015-04-18 (annotated)

Committer:
Hypna
Date:
Sat Apr 18 04:57:20 2015 +0000
Revision:
22:8f726dc175cd
Parent:
19:9f4510646c9e 
stuff

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
Hypna 22:8f726dc175cd 16 bool NavController::isDone() { return map.getPosition() == map.getFinish(); }
jmar11 17:5046b27f5441 17
Hypna 19:9f4510646c9e 18 void NavController::prepReturn()
jmar11 17:5046b27f5441 19 {
Hypna 19:9f4510646c9e 20 stack<R5Map::direction> temp;
jmar11 17:5046b27f5441 21
jmar11 17:5046b27f5441 22 while(!path.empty())
jmar11 17:5046b27f5441 23 {
jmar11 17:5046b27f5441 24 temp.push(path.top());
jmar11 17:5046b27f5441 25 path.pop();
jmar11 17:5046b27f5441 26 }
jmar11 17:5046b27f5441 27
jmar11 17:5046b27f5441 28 path = temp;
jmar11 17:5046b27f5441 29 }
jmar11 17:5046b27f5441 30
Hypna 19:9f4510646c9e 31 R5Map::direction NavController::nextTraceMove()
jmar11 17:5046b27f5441 32 {
Hypna 19:9f4510646c9e 33 R5Map::position newPos = map.getPosition();
Hypna 19:9f4510646c9e 34 R5Map::nextMove;
Hypna 19:9f4510646c9e 35
Hypna 19:9f4510646c9e 36 nextMove = path.top();
Hypna 19:9f4510646c9e 37 path.pop();
Hypna 19:9f4510646c9e 38
Hypna 19:9f4510646c9e 39 switch(nextMove)
Hypna 19:9f4510646c9e 40 {
Hypna 19:9f4510646c9e 41 case NORTH : newPos.y++;
Hypna 19:9f4510646c9e 42 break;
Hypna 19:9f4510646c9e 43 case SOUTH : newPos.y--;
Hypna 19:9f4510646c9e 44 break;
Hypna 19:9f4510646c9e 45 case EAST : newPos.x++;
Hypna 19:9f4510646c9e 46 break;
Hypna 19:9f4510646c9e 47 case WEST : newPos.x--;
Hypna 19:9f4510646c9e 48 }
Hypna 19:9f4510646c9e 49
Hypna 19:9f4510646c9e 50 map.setPosition(newPos);
Hypna 19:9f4510646c9e 51 return nextMove;
Hypna 19:9f4510646c9e 52 }
Hypna 19:9f4510646c9e 53
Hypna 19:9f4510646c9e 54 R5Map::direction NavController::nextExploreMove()
Hypna 19:9f4510646c9e 55 {
Hypna 19:9f4510646c9e 56 R5Map::position current = map.getPosition();
Hypna 19:9f4510646c9e 57 R5Map::position dest = map.getFinish();
jmar11 17:5046b27f5441 58
jmar11 17:5046b27f5441 59 int northWgt = dest.y - current.y;
jmar11 17:5046b27f5441 60 int southWgt = current.y;
jmar11 17:5046b27f5441 61 int eastWgt = current.x;
jmar11 17:5046b27f5441 62 int westWgt = current.x - dest.x;
jmar11 17:5046b27f5441 63
Hypna 19:9f4510646c9e 64 R5Map::direction nextMove;
jmar11 17:5046b27f5441 65
jmar11 17:5046b27f5441 66 if(map.getVisited(NORTH) || map.getCurWall(NORTH) == CLOSED)
jmar11 17:5046b27f5441 67 northWgt = 0;
jmar11 17:5046b27f5441 68
jmar11 17:5046b27f5441 69 if(map.getVisited(SOUTH) || map.getCurWall(SOUTH) == CLOSED)
jmar11 17:5046b27f5441 70 southWgt = 0;
jmar11 17:5046b27f5441 71
jmar11 17:5046b27f5441 72 if(map.getVisited(EAST) || map.getCurWall(EAST) == CLOSED)
jmar11 17:5046b27f5441 73 eastWgt = 0;
jmar11 17:5046b27f5441 74
jmar11 17:5046b27f5441 75 if(map.getVisited(WEST) || map.getCurWall(WEST) == CLOSED)
jmar11 17:5046b27f5441 76 westWgt = 0;
jmar11 17:5046b27f5441 77
jmar11 17:5046b27f5441 78 if(northWgt != 0 || westWgt != 0)
jmar11 17:5046b27f5441 79 {
Hypna 19:9f4510646c9e 80 if(northWgt > westWgt)
jmar11 17:5046b27f5441 81 nextMove = NORTH;
jmar11 17:5046b27f5441 82 else
jmar11 17:5046b27f5441 83 nextMove = WEST;
jmar11 17:5046b27f5441 84
jmar11 17:5046b27f5441 85 path.push(nextMove);
jmar11 17:5046b27f5441 86 }
jmar11 17:5046b27f5441 87 else if(southWgt != 0 || eastWgt != 0)
jmar11 17:5046b27f5441 88 {
jmar11 17:5046b27f5441 89 if(southWgt > eastWgt)
jmar11 17:5046b27f5441 90 nextMove = SOUTH;
jmar11 17:5046b27f5441 91 else
jmar11 17:5046b27f5441 92 nextMove = EAST;
jmar11 17:5046b27f5441 93
jmar11 17:5046b27f5441 94 path.push(nextMove);
jmar11 17:5046b27f5441 95 }
jmar11 17:5046b27f5441 96 else
jmar11 17:5046b27f5441 97 {
Hypna 19:9f4510646c9e 98 R5Map::direction lastMove = path.top();
jmar11 17:5046b27f5441 99 path.pop();
jmar11 17:5046b27f5441 100
jmar11 17:5046b27f5441 101 switch(lastMove)
jmar11 17:5046b27f5441 102 {
jmar11 17:5046b27f5441 103 case NORTH : nextMove = SOUTH;
jmar11 17:5046b27f5441 104 break;
jmar11 17:5046b27f5441 105 case SOUTH : nextMove = NORTH;
jmar11 17:5046b27f5441 106 break;
jmar11 17:5046b27f5441 107 case EAST : nextMove = WEST;
jmar11 17:5046b27f5441 108 break;
jmar11 17:5046b27f5441 109 case WEST : nextMove = EAST;
jmar11 17:5046b27f5441 110 }
jmar11 17:5046b27f5441 111 }
jmar11 17:5046b27f5441 112
jmar11 17:5046b27f5441 113 return nextMove;
jmar11 17:5046b27f5441 114 }
jmar11 17:5046b27f5441 115
jmar11 17:5046b27f5441 116 void NavController::updateMap()
jmar11 17:5046b27f5441 117 {
jmar11 17:5046b27f5441 118 if(!map.getCurVisited())
jmar11 17:5046b27f5441 119 {
jmar11 17:5046b27f5441 120 readSensor(NORTH) ? map.setCurWall(NORTH, CLOSED) : map.setCurWall(NORTH, OPEN);
jmar11 17:5046b27f5441 121 readSensor(SOUTH) ? map.setCurWall(SOUTH, CLOSED) : map.setCurWall(SOUTH, OPEN);
jmar11 17:5046b27f5441 122 readSensor(EAST) ? map.setCurWall(EAST, CLOSED) : map.setCurWall(EAST, OPEN);
jmar11 17:5046b27f5441 123 readSensor(WEST) ? map.setCurWall(WEST, CLOSED) : map.setCurWall(WEST, OPEN);
jmar11 17:5046b27f5441 124 }
jmar11 17:5046b27f5441 125 }
jmar11 17:5046b27f5441 126
Hypna 19:9f4510646c9e 127 bool NavController::readSensor(R5Map::direction wall)
jmar11 17:5046b27f5441 128 {
Hypna 19:9f4510646c9e 129 int diff = wall - orient;
jmar11 17:5046b27f5441 130
Hypna 19:9f4510646c9e 131 switch(diff)
jmar11 17:5046b27f5441 132 {
jmar11 17:5046b27f5441 133 case 0:
jmar11 17:5046b27f5441 134 digitalRead(SENS_FORWARD);
jmar11 17:5046b27f5441 135 case 1:
jmar11 17:5046b27f5441 136 digitalRead(SENS_RIGHT);
jmar11 17:5046b27f5441 137 case 2:
jmar11 17:5046b27f5441 138 digitalRead(SENS_BACK);
jmar11 17:5046b27f5441 139 case 3:
jmar11 17:5046b27f5441 140 digitalRead(SENS_LEFT);
jmar11 17:5046b27f5441 141 }
Hypna 19:9f4510646c9e 142 }