Texas State IEEE / navigation

navigation updated with completed dijkstra's algo

Dependents:   R5 2016 Robotics Team 1

navigation.cpp@9:e24b50c1741b, 2016-04-08 (annotated)

Committer:
j_j205
Date:
Fri Apr 08 05:00:24 2016 +0000
Revision:
9:e24b50c1741b
Parent:
8:290a110bcf0e
Child:
10:23e21c19b48a
latest

Who changed what in which revision?

UserRevisionLine numberNew contents of line
j_j205 2:17bd430aeca1 1 #include "StepperDrive.h"
j_j205 6:d2da4d4b5112 2 #include "LongRangeSensor.h"
j_j205 0:fd72f6df078c 3 #include "navigation.h"
j_j205 0:fd72f6df078c 4 #include "stdint.h"
j_j205 2:17bd430aeca1 5 #include <cmath>
j_j205 0:fd72f6df078c 6 #include <fstream>
j_j205 0:fd72f6df078c 7 #include <stack>
j_j205 0:fd72f6df078c 8 #include <set>
j_j205 0:fd72f6df078c 9
j_j205 0:fd72f6df078c 10 // FUNCTION:
j_j205 0:fd72f6df078c 11 // Navigation(int size = DEFAULT_SIZE)
j_j205 0:fd72f6df078c 12 // IN-PARAMETERS:
j_j205 0:fd72f6df078c 13 // Number of initial vertices (DEFAULT = 1)
j_j205 0:fd72f6df078c 14 // OUT-PARAMETERS:
j_j205 0:fd72f6df078c 15 // None
j_j205 0:fd72f6df078c 16 // DESCRIPTION:
j_j205 0:fd72f6df078c 17 // Default and 1-parameter constructor.
j_j205 6:d2da4d4b5112 18 Navigation::Navigation(Serial &_pc, StepperDrive &_drive,
j_j205 6:d2da4d4b5112 19 LongRangeSensor &_longRangeL, LongRangeSensor &_longRangeR, DigitalOut &_led_red, DigitalOut &_led_green,
j_j205 6:d2da4d4b5112 20 int size = DEFAULT_SIZE) : pc(_pc), drive(_drive),
j_j205 6:d2da4d4b5112 21 longRangeL(_longRangeL), longRangeR(_longRangeR), led_red(_led_red), led_green(_led_green),
j_j205 6:d2da4d4b5112 22 vertex(DEFAULT_VERTEX), angle(DEFAULT_ANGLE)
j_j205 0:fd72f6df078c 23 {
j_j205 0:fd72f6df078c 24 graph.resize(size);
j_j205 6:d2da4d4b5112 25 distLocalL = 0.0;
j_j205 0:fd72f6df078c 26 }
j_j205 0:fd72f6df078c 27
j_j205 0:fd72f6df078c 28 // FUNCTION:
j_j205 0:fd72f6df078c 29 // addGraphNode(uint16_t src, uint16_t target, uint16_t dist,
j_j205 0:fd72f6df078c 30 // uint16_t angle)
j_j205 0:fd72f6df078c 31 // IN-PARAMETERS:
j_j205 0:fd72f6df078c 32 // Source node (uint16_t), target node (uint16_t), edge distance
j_j205 0:fd72f6df078c 33 // (uint16_t), and angle (uint16_t)
j_j205 0:fd72f6df078c 34 // OUT-PARAMETERS:
j_j205 0:fd72f6df078c 35 // None
j_j205 0:fd72f6df078c 36 // DESCRIPTION:
j_j205 0:fd72f6df078c 37 // Adds a graph node to the graph.
j_j205 6:d2da4d4b5112 38 void Navigation::addGraphNode(uint8_t src, uint8_t target, float dist,
j_j205 2:17bd430aeca1 39 float angle)
j_j205 0:fd72f6df078c 40 {
j_j205 0:fd72f6df078c 41 graph[src].push_back(graphNode(target, dist, angle));
j_j205 0:fd72f6df078c 42 }
j_j205 0:fd72f6df078c 43
j_j205 0:fd72f6df078c 44 // FUNCTION:
j_j205 0:fd72f6df078c 45 // addGraphNode(uint16_t src, uint16_t target, uint16_t dist,
j_j205 0:fd72f6df078c 46 // uint16_t angle)
j_j205 0:fd72f6df078c 47 // IN-PARAMETERS:
j_j205 0:fd72f6df078c 48 // Source node (uint16_t)
j_j205 0:fd72f6df078c 49 // OUT-PARAMETERS:
j_j205 0:fd72f6df078c 50 // None
j_j205 0:fd72f6df078c 51 // DESCRIPTION:
j_j205 0:fd72f6df078c 52 // Adds a graph node to the graph, with target, dist, and angle
j_j205 0:fd72f6df078c 53 // initialized to infinity
j_j205 6:d2da4d4b5112 54 void Navigation::addGraphNode(uint8_t src)
j_j205 0:fd72f6df078c 55 {
j_j205 0:fd72f6df078c 56 graph[src].push_back(graphNode());
j_j205 0:fd72f6df078c 57 }
j_j205 0:fd72f6df078c 58
j_j205 0:fd72f6df078c 59 // FUNCTION:
j_j205 0:fd72f6df078c 60 // int loadMap(char* infile);
j_j205 0:fd72f6df078c 61 // IN-PARAMETERS:
j_j205 0:fd72f6df078c 62 // Input file name (*char[])
j_j205 0:fd72f6df078c 63 // OUT-PARAMETERS:
j_j205 0:fd72f6df078c 64 // Returns 1 if file doesn't open
j_j205 0:fd72f6df078c 65 // Returns 0 if file opens succesfully
j_j205 0:fd72f6df078c 66 // DESCRIPTION:
j_j205 0:fd72f6df078c 67 // Loads contents of a file as a weighted graph. First entry in the file
j_j205 0:fd72f6df078c 68 // should be the number of vertices. All subsequent entries indicate edges
j_j205 0:fd72f6df078c 69 // by listing a source vertex, a target vertex, the distance between them
j_j205 0:fd72f6df078c 70 // and the angle from source to target (0 - 359 degrees) . Only edges
j_j205 0:fd72f6df078c 71 // are listed. Assumes that inverse angles/edges are included in file.
j_j205 0:fd72f6df078c 72 int Navigation::loadMap(char* inputFile)
j_j205 0:fd72f6df078c 73 {
j_j205 0:fd72f6df078c 74 std::ifstream inFile;
j_j205 0:fd72f6df078c 75 inFile.open(inputFile);
j_j205 0:fd72f6df078c 76 if (!inFile)
j_j205 0:fd72f6df078c 77 return 1; // returns 1 if file fails to open
j_j205 0:fd72f6df078c 78
j_j205 0:fd72f6df078c 79 int vertices;
j_j205 0:fd72f6df078c 80 inFile >> vertices; // reads first entry as number of vertices
j_j205 0:fd72f6df078c 81 graph.resize(vertices); // resizes vector as necessary
j_j205 0:fd72f6df078c 82
j_j205 6:d2da4d4b5112 83 uint8_t src;
j_j205 6:d2da4d4b5112 84 uint8_t target;
j_j205 2:17bd430aeca1 85 float dist;
j_j205 2:17bd430aeca1 86 float angle;
j_j205 0:fd72f6df078c 87
j_j205 0:fd72f6df078c 88 while (!inFile.eof() )
j_j205 0:fd72f6df078c 89 {
j_j205 0:fd72f6df078c 90 inFile >> src;
j_j205 0:fd72f6df078c 91 inFile >> target;
j_j205 0:fd72f6df078c 92 inFile >> dist;
j_j205 0:fd72f6df078c 93 inFile >> angle;
j_j205 0:fd72f6df078c 94
j_j205 0:fd72f6df078c 95 addGraphNode(src, target, dist, angle);
j_j205 0:fd72f6df078c 96 } // loads all data from file into graph
j_j205 0:fd72f6df078c 97
j_j205 0:fd72f6df078c 98 return 0;
j_j205 0:fd72f6df078c 99 }
j_j205 0:fd72f6df078c 100
j_j205 0:fd72f6df078c 101 // FUNCTION:
j_j205 0:fd72f6df078c 102 // void getShortestPath(uint16_t src, uint16_t destination)
j_j205 0:fd72f6df078c 103 // IN-PARAMETERS:
j_j205 0:fd72f6df078c 104 // Target vertex (uint16_t)
j_j205 0:fd72f6df078c 105 // OUT-PARAMETERS:
j_j205 0:fd72f6df078c 106 // None
j_j205 0:fd72f6df078c 107 // DESCRIPTION:
j_j205 0:fd72f6df078c 108 // Implementation of Dijkstra's algorithm. Uses current vertex as source
j_j205 0:fd72f6df078c 109 // and calculates shortest path information for all other vertices.
j_j205 0:fd72f6df078c 110 // Subroutine ends when shortest path for target vertex has been found
j_j205 0:fd72f6df078c 111 // and then loads the path into route.
j_j205 6:d2da4d4b5112 112 void Navigation::getShortestPath(uint8_t destination)
j_j205 0:fd72f6df078c 113 {
j_j205 6:d2da4d4b5112 114 uint8_t src = vertex;
j_j205 6:d2da4d4b5112 115 uint8_t v; // stores temporary vertex
j_j205 0:fd72f6df078c 116 int n = graph.size();
j_j205 0:fd72f6df078c 117 minDistance.clear();
j_j205 0:fd72f6df078c 118 minDistance.resize(n, MAX_DIST);
j_j205 0:fd72f6df078c 119 minDistance[src] = 0;
j_j205 0:fd72f6df078c 120 previous.clear();
j_j205 0:fd72f6df078c 121 previous.resize(n, -1);
j_j205 6:d2da4d4b5112 122 std::set<std::pair<float, uint8_t> > vertex_queue;
j_j205 0:fd72f6df078c 123 vertex_queue.insert(std::make_pair(minDistance[src], src));
j_j205 0:fd72f6df078c 124
j_j205 0:fd72f6df078c 125 while (!vertex_queue.empty())
j_j205 0:fd72f6df078c 126 {
j_j205 2:17bd430aeca1 127 float dist = vertex_queue.begin()->first;
j_j205 6:d2da4d4b5112 128 uint8_t u = vertex_queue.begin()->second;
j_j205 0:fd72f6df078c 129 vertex_queue.erase(vertex_queue.begin());
j_j205 0:fd72f6df078c 130
j_j205 0:fd72f6df078c 131 // Visit each edge exiting u
j_j205 0:fd72f6df078c 132 const std::vector<graphNode> &neighbors = graph[u];
j_j205 0:fd72f6df078c 133 for (std::vector<graphNode>::const_iterator neighbor_iter = neighbors.begin();
j_j205 0:fd72f6df078c 134 neighbor_iter != neighbors.end();
j_j205 0:fd72f6df078c 135 neighbor_iter++)
j_j205 0:fd72f6df078c 136 {
j_j205 0:fd72f6df078c 137 v = neighbor_iter->neighbor;
j_j205 2:17bd430aeca1 138 float weight = neighbor_iter->distance;
j_j205 2:17bd430aeca1 139 float distance_through_u = dist + weight;
j_j205 0:fd72f6df078c 140
j_j205 0:fd72f6df078c 141 if (distance_through_u < minDistance[v])
j_j205 0:fd72f6df078c 142 {
j_j205 0:fd72f6df078c 143 vertex_queue.erase(std::make_pair(minDistance[v], v));
j_j205 0:fd72f6df078c 144
j_j205 0:fd72f6df078c 145 minDistance[v] = distance_through_u;
j_j205 0:fd72f6df078c 146 previous[v] = u;
j_j205 0:fd72f6df078c 147 vertex_queue.insert(std::make_pair(minDistance[v], v));
j_j205 0:fd72f6df078c 148 }
j_j205 0:fd72f6df078c 149 if (v == destination)
j_j205 0:fd72f6df078c 150 break;
j_j205 0:fd72f6df078c 151 }
j_j205 0:fd72f6df078c 152 if (v == destination)
j_j205 0:fd72f6df078c 153 break;
j_j205 0:fd72f6df078c 154 }
j_j205 0:fd72f6df078c 155 while (v != src)
j_j205 0:fd72f6df078c 156 {
j_j205 0:fd72f6df078c 157 route.push(v);
j_j205 0:fd72f6df078c 158 v = previous[v];
j_j205 0:fd72f6df078c 159 }
j_j205 0:fd72f6df078c 160 }
j_j205 0:fd72f6df078c 161
j_j205 1:a53d97b74fab 162 // FUNCTION:
j_j205 1:a53d97b74fab 163 // void executeRoute()
j_j205 1:a53d97b74fab 164 // IN-PARAMETERS:
j_j205 1:a53d97b74fab 165 // Address of left and right steppers (StepperMotor &)
j_j205 1:a53d97b74fab 166 // OUT-PARAMETERS:
j_j205 1:a53d97b74fab 167 // None
j_j205 1:a53d97b74fab 168 // DESCRIPTION:
j_j205 1:a53d97b74fab 169 // Sends command to steppers to execute route.
j_j205 6:d2da4d4b5112 170 void Navigation::executeRoute()
j_j205 1:a53d97b74fab 171 {
j_j205 6:d2da4d4b5112 172 uint8_t target;
j_j205 2:17bd430aeca1 173 float targetAngle;
j_j205 2:17bd430aeca1 174 float targetDist;
j_j205 2:17bd430aeca1 175 float differenceAngle;
j_j205 4:a5d44517c65c 176
j_j205 1:a53d97b74fab 177 while (!route.empty() )
j_j205 1:a53d97b74fab 178 {
j_j205 1:a53d97b74fab 179 target = route.top();
j_j205 1:a53d97b74fab 180 route.pop();
j_j205 4:a5d44517c65c 181
j_j205 2:17bd430aeca1 182 for (int i = 0; i < graph[vertex].size(); i++)
j_j205 2:17bd430aeca1 183 {
j_j205 2:17bd430aeca1 184 if (graph[vertex][i].neighbor == target)
j_j205 2:17bd430aeca1 185 {
j_j205 2:17bd430aeca1 186 targetAngle = graph[vertex][i].angle;
j_j205 2:17bd430aeca1 187 targetDist = graph[vertex][i].distance;
j_j205 2:17bd430aeca1 188 break;
j_j205 2:17bd430aeca1 189 }
j_j205 2:17bd430aeca1 190 }
j_j205 4:a5d44517c65c 191
j_j205 2:17bd430aeca1 192 if (targetAngle != angle)
j_j205 2:17bd430aeca1 193 {
j_j205 2:17bd430aeca1 194 differenceAngle = targetAngle - angle;
j_j205 4:a5d44517c65c 195
j_j205 2:17bd430aeca1 196 if (abs(differenceAngle) > 180)
j_j205 2:17bd430aeca1 197 {
j_j205 6:d2da4d4b5112 198 if (differenceAngle < 0)
j_j205 2:17bd430aeca1 199 differenceAngle = 360 - abs(differenceAngle);
j_j205 2:17bd430aeca1 200 else
j_j205 2:17bd430aeca1 201 differenceAngle = -(360 - differenceAngle);
j_j205 2:17bd430aeca1 202 }
j_j205 4:a5d44517c65c 203
j_j205 2:17bd430aeca1 204 else
j_j205 2:17bd430aeca1 205 {
j_j205 2:17bd430aeca1 206 /* do nothing */
j_j205 2:17bd430aeca1 207 }
j_j205 4:a5d44517c65c 208
j_j205 2:17bd430aeca1 209 /* send -(differenceAngle) to motors */
j_j205 3:27cc2bacd6af 210 int returnVal = drive.move(0, ((-differenceAngle)*(PI / 180.0)) );
j_j205 4:a5d44517c65c 211 pc.printf("\nChanged angle %f degrees to %f", differenceAngle, targetAngle);
j_j205 4:a5d44517c65c 212
j_j205 2:17bd430aeca1 213 // wait for move to complete
j_j205 2:17bd430aeca1 214 while(!drive.isMoveDone())
j_j205 2:17bd430aeca1 215 {
j_j205 3:27cc2bacd6af 216 wait(1e-6);
j_j205 2:17bd430aeca1 217 }
j_j205 7:f7489797746b 218 /*
j_j205 6:d2da4d4b5112 219 distLocalL = longRangeL.distInchesLOne();
j_j205 6:d2da4d4b5112 220 distLocalR = longRangeR.distInchesROne();
j_j205 6:d2da4d4b5112 221 pc.printf("\n distLocalL = %f", distLocalL);
j_j205 6:d2da4d4b5112 222 pc.printf("\n distLocalR = %f", distLocalR);
j_j205 7:f7489797746b 223 if ((distLocalL <= 8.7) && (distLocalL >= 3.0)) // wall in range
j_j205 6:d2da4d4b5112 224 {
j_j205 6:d2da4d4b5112 225 localizeRight();
j_j205 6:d2da4d4b5112 226 }
j_j205 6:d2da4d4b5112 227
j_j205 6:d2da4d4b5112 228 else if ((distLocalR <= 10.5) && (distLocalR >= 5.0)) // wall in range
j_j205 6:d2da4d4b5112 229 {
j_j205 6:d2da4d4b5112 230 localizeLeft();
j_j205 7:f7489797746b 231 }*/
j_j205 2:17bd430aeca1 232 }
j_j205 4:a5d44517c65c 233
j_j205 2:17bd430aeca1 234 /* send targetDist to motors */
j_j205 3:27cc2bacd6af 235 int returnVal = drive.move(targetDist, 0);
j_j205 2:17bd430aeca1 236 pc.printf("\nMoved forward a distance of %f to node %i", targetDist, target);
j_j205 3:27cc2bacd6af 237
j_j205 2:17bd430aeca1 238 // wait for move to complete
j_j205 2:17bd430aeca1 239 while(!drive.isMoveDone())
j_j205 2:17bd430aeca1 240 {
j_j205 3:27cc2bacd6af 241 wait(1e-6);
j_j205 2:17bd430aeca1 242 }
j_j205 4:a5d44517c65c 243
j_j205 2:17bd430aeca1 244 vertex = target;
j_j205 2:17bd430aeca1 245 angle = targetAngle;
j_j205 6:d2da4d4b5112 246
j_j205 6:d2da4d4b5112 247 distLocalL = longRangeL.distInchesLOne();
j_j205 6:d2da4d4b5112 248 distLocalR = longRangeR.distInchesROne();
j_j205 6:d2da4d4b5112 249 pc.printf("\n distLocalL = %f", distLocalL);
j_j205 8:290a110bcf0e 250 pc.printf("\n distLocalR = %f", distLocalR);
j_j205 8:290a110bcf0e 251 if (distLocalL <= 18.0) // wall in range
j_j205 7:f7489797746b 252 {
j_j205 8:290a110bcf0e 253 newLocalizeRight();
j_j205 7:f7489797746b 254 }
j_j205 6:d2da4d4b5112 255
j_j205 8:290a110bcf0e 256 else if (distLocalR <= 13.0) // wall in range
j_j205 7:f7489797746b 257 {
j_j205 8:290a110bcf0e 258 newLocalizeLeft();
j_j205 7:f7489797746b 259 }
j_j205 1:a53d97b74fab 260 }
j_j205 1:a53d97b74fab 261 }
j_j205 1:a53d97b74fab 262
j_j205 0:fd72f6df078c 263 // utility function
j_j205 0:fd72f6df078c 264 void Navigation::printPrevious(Serial &pc)
j_j205 0:fd72f6df078c 265 {
j_j205 0:fd72f6df078c 266 for(int i = 0; i < previous.size(); i++)
j_j205 0:fd72f6df078c 267 {
j_j205 2:17bd430aeca1 268 pc.printf("Node %f is prev to node %f\n", previous[i], i);
j_j205 0:fd72f6df078c 269 }
j_j205 0:fd72f6df078c 270 }
j_j205 0:fd72f6df078c 271
j_j205 0:fd72f6df078c 272 // utility function
j_j205 0:fd72f6df078c 273 void Navigation::printRoute(Serial &pc)
j_j205 0:fd72f6df078c 274 {
j_j205 0:fd72f6df078c 275 while (!route.empty() )
j_j205 0:fd72f6df078c 276 {
j_j205 0:fd72f6df078c 277 pc.printf("Go to node %i\n", route.top() );
j_j205 0:fd72f6df078c 278 route.pop();
j_j205 0:fd72f6df078c 279 }
j_j205 0:fd72f6df078c 280 }
j_j205 0:fd72f6df078c 281
j_j205 0:fd72f6df078c 282 // utility function
j_j205 0:fd72f6df078c 283 void Navigation::printGraph(Serial &pc)
j_j205 0:fd72f6df078c 284 {
j_j205 0:fd72f6df078c 285 for (int i = 0; i < graph.size(); i++)
j_j205 0:fd72f6df078c 286 for (int j = 0; j < graph[i].size(); j++)
j_j205 0:fd72f6df078c 287 {
j_j205 2:17bd430aeca1 288 pc.printf("Node %i to %i = dist - %f, angle - %f\n", i, j, graph[i][j].distance, graph[i][j].angle);
j_j205 0:fd72f6df078c 289 }
j_j205 5:d0954e0aecc9 290 }
j_j205 6:d2da4d4b5112 291
j_j205 6:d2da4d4b5112 292 // FUNCTION:
j_j205 6:d2da4d4b5112 293 // void localizeRight()
j_j205 6:d2da4d4b5112 294 // IN-PARAMETERS:
j_j205 6:d2da4d4b5112 295 // None
j_j205 6:d2da4d4b5112 296 // OUT-PARAMETERS:
j_j205 6:d2da4d4b5112 297 // None
j_j205 6:d2da4d4b5112 298 // DESCRIPTION:
j_j205 6:d2da4d4b5112 299 // Using sensor longRangeL this method will localize to the wall
j_j205 6:d2da4d4b5112 300 // to the right
j_j205 6:d2da4d4b5112 301 void Navigation::localizeRight()
j_j205 6:d2da4d4b5112 302 {
j_j205 7:f7489797746b 303 if (distLocalL >= 7.8) // very close left, turn more right
j_j205 7:f7489797746b 304 {
j_j205 7:f7489797746b 305 drive.move(0, (5.0)*(3.14159 / 180.0));
j_j205 7:f7489797746b 306 // wait for move to complete
j_j205 7:f7489797746b 307 while(!drive.isMoveDone())
j_j205 7:f7489797746b 308 {
j_j205 7:f7489797746b 309 wait(1e-6);
j_j205 7:f7489797746b 310 }
j_j205 7:f7489797746b 311 pc.printf("\n Adjusted angle more right, in localizeRight");
j_j205 7:f7489797746b 312 }
j_j205 7:f7489797746b 313
j_j205 7:f7489797746b 314 else if (distLocalL >= 6.9) // too close left, turn right
j_j205 6:d2da4d4b5112 315 {
j_j205 6:d2da4d4b5112 316 drive.move(0, (2.0)*(3.14159 / 180.0));
j_j205 6:d2da4d4b5112 317 // wait for move to complete
j_j205 6:d2da4d4b5112 318 while(!drive.isMoveDone())
j_j205 6:d2da4d4b5112 319 {
j_j205 6:d2da4d4b5112 320 wait(1e-6);
j_j205 6:d2da4d4b5112 321 }
j_j205 6:d2da4d4b5112 322 pc.printf("\n Adjusted angle right, in localizeRight");
j_j205 6:d2da4d4b5112 323 }
j_j205 6:d2da4d4b5112 324
j_j205 7:f7489797746b 325 else if (distLocalL <= 5.5) // too close right, turn left
j_j205 6:d2da4d4b5112 326 {
j_j205 6:d2da4d4b5112 327 drive.move(0, (-2.0)*(3.14159 / 180.0));
j_j205 6:d2da4d4b5112 328 // wait for move to complete
j_j205 6:d2da4d4b5112 329 while(!drive.isMoveDone())
j_j205 6:d2da4d4b5112 330 {
j_j205 6:d2da4d4b5112 331 wait(1e-6);
j_j205 6:d2da4d4b5112 332 }
j_j205 6:d2da4d4b5112 333 pc.printf("\n Adjusted angle left , in localizeRight");
j_j205 6:d2da4d4b5112 334 }
j_j205 6:d2da4d4b5112 335
j_j205 6:d2da4d4b5112 336 else
j_j205 6:d2da4d4b5112 337 {
j_j205 6:d2da4d4b5112 338 /* do nothing */
j_j205 6:d2da4d4b5112 339 pc.printf("\n No need to adjust, in localizeRight");
j_j205 6:d2da4d4b5112 340 }
j_j205 6:d2da4d4b5112 341 }
j_j205 6:d2da4d4b5112 342
j_j205 6:d2da4d4b5112 343 // FUNCTION:
j_j205 6:d2da4d4b5112 344 // void localizeLeft()
j_j205 6:d2da4d4b5112 345 // IN-PARAMETERS:
j_j205 6:d2da4d4b5112 346 // None
j_j205 6:d2da4d4b5112 347 // OUT-PARAMETERS:
j_j205 6:d2da4d4b5112 348 // None
j_j205 6:d2da4d4b5112 349 // DESCRIPTION:
j_j205 6:d2da4d4b5112 350 // Using sensor longRangeR this method will localize to the wall
j_j205 6:d2da4d4b5112 351 // to the left during stretches of forward motion where robot
j_j205 6:d2da4d4b5112 352 // should remain xxx distance from the left wall.
j_j205 6:d2da4d4b5112 353 void Navigation::localizeLeft()
j_j205 6:d2da4d4b5112 354 {
j_j205 6:d2da4d4b5112 355 if (distLocalR >= 9.7) // too close right, turn left
j_j205 6:d2da4d4b5112 356 {
j_j205 6:d2da4d4b5112 357 drive.move(0.0, (-2.0)*(3.14159 / 180.0));
j_j205 6:d2da4d4b5112 358 // wait for move to complete
j_j205 6:d2da4d4b5112 359 while(!drive.isMoveDone())
j_j205 6:d2da4d4b5112 360 {
j_j205 6:d2da4d4b5112 361 wait(1e-6);
j_j205 6:d2da4d4b5112 362 }
j_j205 6:d2da4d4b5112 363 pc.printf("\n Adjusted angle left, in localizeLeft");
j_j205 6:d2da4d4b5112 364 }
j_j205 6:d2da4d4b5112 365
j_j205 6:d2da4d4b5112 366 else if (distLocalR <= 8.3) // too close left, turn right
j_j205 6:d2da4d4b5112 367 {
j_j205 6:d2da4d4b5112 368 drive.move(0.0, (2.0)*(3.14159 / 180.0));
j_j205 6:d2da4d4b5112 369 // wait for move to complete
j_j205 6:d2da4d4b5112 370 while(!drive.isMoveDone())
j_j205 6:d2da4d4b5112 371 {
j_j205 6:d2da4d4b5112 372 wait(1e-6);
j_j205 6:d2da4d4b5112 373 }
j_j205 6:d2da4d4b5112 374 pc.printf("\n Adjusted angle right, in localizeLeft");
j_j205 6:d2da4d4b5112 375 }
j_j205 6:d2da4d4b5112 376
j_j205 6:d2da4d4b5112 377 else
j_j205 6:d2da4d4b5112 378 {
j_j205 6:d2da4d4b5112 379 /* do nothing */
j_j205 6:d2da4d4b5112 380 pc.printf("\n No need to adjust, in localizeLeft");
j_j205 6:d2da4d4b5112 381 }
j_j205 6:d2da4d4b5112 382 }
j_j205 6:d2da4d4b5112 383
j_j205 6:d2da4d4b5112 384 // FUNCTION:
j_j205 6:d2da4d4b5112 385 // void localizeRightReverse()
j_j205 6:d2da4d4b5112 386 // IN-PARAMETERS:
j_j205 6:d2da4d4b5112 387 // None
j_j205 6:d2da4d4b5112 388 // OUT-PARAMETERS:
j_j205 6:d2da4d4b5112 389 // None
j_j205 6:d2da4d4b5112 390 // DESCRIPTION:
j_j205 6:d2da4d4b5112 391 // Using sensor longRangeL this method will localize to the wall
j_j205 6:d2da4d4b5112 392 // to the right
j_j205 6:d2da4d4b5112 393 void Navigation::localizeRightReverse()
j_j205 6:d2da4d4b5112 394 {
j_j205 6:d2da4d4b5112 395 if (distLocalL >= 7.2) // too close left, turn left
j_j205 6:d2da4d4b5112 396 {
j_j205 6:d2da4d4b5112 397 drive.move(0, (-2.0)*(3.14159 / 180.0));
j_j205 6:d2da4d4b5112 398 // wait for move to complete
j_j205 6:d2da4d4b5112 399 while(!drive.isMoveDone())
j_j205 6:d2da4d4b5112 400 {
j_j205 6:d2da4d4b5112 401 wait(1e-6);
j_j205 6:d2da4d4b5112 402 }
j_j205 6:d2da4d4b5112 403 pc.printf("\n Adjusted angle left, in localizeRightReverse");
j_j205 6:d2da4d4b5112 404 }
j_j205 6:d2da4d4b5112 405
j_j205 6:d2da4d4b5112 406 else if (distLocalL <= 6.2) // too close right, turn right
j_j205 6:d2da4d4b5112 407 {
j_j205 6:d2da4d4b5112 408 drive.move(0, (2.0)*(3.14159 / 180.0));
j_j205 6:d2da4d4b5112 409 // wait for move to complete
j_j205 6:d2da4d4b5112 410 while(!drive.isMoveDone())
j_j205 6:d2da4d4b5112 411 {
j_j205 6:d2da4d4b5112 412 wait(1e-6);
j_j205 6:d2da4d4b5112 413 }
j_j205 6:d2da4d4b5112 414 pc.printf("\n Adjusted angle right, in localizeRightReverse");
j_j205 6:d2da4d4b5112 415 }
j_j205 6:d2da4d4b5112 416
j_j205 6:d2da4d4b5112 417 else
j_j205 6:d2da4d4b5112 418 {
j_j205 6:d2da4d4b5112 419 /* do nothing */
j_j205 6:d2da4d4b5112 420 pc.printf("\n No need to adjust, in localizeRightReverse");
j_j205 6:d2da4d4b5112 421 }
j_j205 6:d2da4d4b5112 422 }
j_j205 6:d2da4d4b5112 423
j_j205 6:d2da4d4b5112 424 // FUNCTION:
j_j205 6:d2da4d4b5112 425 // void localizeLeftReverse()
j_j205 6:d2da4d4b5112 426 // IN-PARAMETERS:
j_j205 6:d2da4d4b5112 427 // None
j_j205 6:d2da4d4b5112 428 // OUT-PARAMETERS:
j_j205 6:d2da4d4b5112 429 // None
j_j205 6:d2da4d4b5112 430 // DESCRIPTION:
j_j205 6:d2da4d4b5112 431 // Using sensor longRangeR this method will localize to the wall
j_j205 6:d2da4d4b5112 432 // to the left during stretches of forward motion where robot
j_j205 6:d2da4d4b5112 433 // should remain xxx distance from the left wall.
j_j205 6:d2da4d4b5112 434 void Navigation::localizeLeftReverse()
j_j205 6:d2da4d4b5112 435 {
j_j205 6:d2da4d4b5112 436 if (distLocalR >= 9.7) // toos close right, turn right
j_j205 6:d2da4d4b5112 437 {
j_j205 6:d2da4d4b5112 438 drive.move(0.0, (2.0)*(3.14159 / 180.0));
j_j205 6:d2da4d4b5112 439 // wait for move to complete
j_j205 6:d2da4d4b5112 440 while(!drive.isMoveDone())
j_j205 6:d2da4d4b5112 441 {
j_j205 6:d2da4d4b5112 442 wait(1e-6);
j_j205 6:d2da4d4b5112 443 }
j_j205 6:d2da4d4b5112 444 pc.printf("\n Adjusted angle right, in localizeLeftReverse");
j_j205 6:d2da4d4b5112 445 }
j_j205 6:d2da4d4b5112 446
j_j205 6:d2da4d4b5112 447 else if (distLocalR <= 8.3) // too close left, turn left
j_j205 6:d2da4d4b5112 448 {
j_j205 6:d2da4d4b5112 449 drive.move(0.0, (-2.0)*(3.14159 / 180.0));
j_j205 6:d2da4d4b5112 450 // wait for move to complete
j_j205 6:d2da4d4b5112 451 while(!drive.isMoveDone())
j_j205 6:d2da4d4b5112 452 {
j_j205 6:d2da4d4b5112 453 wait(1e-6);
j_j205 6:d2da4d4b5112 454 }
j_j205 6:d2da4d4b5112 455 pc.printf("\n Adjusted angle left, in localizeLeftReverse");
j_j205 6:d2da4d4b5112 456 }
j_j205 6:d2da4d4b5112 457
j_j205 6:d2da4d4b5112 458 else
j_j205 6:d2da4d4b5112 459 {
j_j205 6:d2da4d4b5112 460 /* do nothing */
j_j205 6:d2da4d4b5112 461 pc.printf("\n No need to adjust, in localizeLeftReverse");
j_j205 6:d2da4d4b5112 462 }
j_j205 6:d2da4d4b5112 463 }
j_j205 7:f7489797746b 464
j_j205 7:f7489797746b 465 // FUNCTION:
j_j205 7:f7489797746b 466 // void newLocalize()
j_j205 7:f7489797746b 467 // IN-PARAMETERS:
j_j205 7:f7489797746b 468 // None
j_j205 7:f7489797746b 469 // OUT-PARAMETERS:
j_j205 7:f7489797746b 470 // None
j_j205 7:f7489797746b 471 // DESCRIPTION:
j_j205 7:f7489797746b 472 // Uses difference between sensors to adjust angle
j_j205 7:f7489797746b 473 void Navigation::newLocalize()
j_j205 7:f7489797746b 474 {
j_j205 7:f7489797746b 475 float k = 2.5;
j_j205 7:f7489797746b 476 float angleAdjust = k*(distLocalL - distLocalR);
j_j205 7:f7489797746b 477
j_j205 8:290a110bcf0e 478 drive.move(0, angleAdjust*(PI / 180.0));
j_j205 8:290a110bcf0e 479 // wait for move to complete
j_j205 8:290a110bcf0e 480 while(!drive.isMoveDone())
j_j205 8:290a110bcf0e 481 {
j_j205 8:290a110bcf0e 482 wait(1e-6);
j_j205 8:290a110bcf0e 483 }
j_j205 8:290a110bcf0e 484 angle += angleAdjust;
j_j205 8:290a110bcf0e 485 pc.printf("\n Adjusted angle by %f in newLocalize", angleAdjust);
j_j205 8:290a110bcf0e 486 }
j_j205 8:290a110bcf0e 487
j_j205 8:290a110bcf0e 488 // FUNCTION:
j_j205 8:290a110bcf0e 489 // void newLocalizeRight()
j_j205 8:290a110bcf0e 490 // IN-PARAMETERS:
j_j205 8:290a110bcf0e 491 // None
j_j205 8:290a110bcf0e 492 // OUT-PARAMETERS:
j_j205 8:290a110bcf0e 493 // None
j_j205 8:290a110bcf0e 494 // DESCRIPTION:
j_j205 8:290a110bcf0e 495 // Left sensor to adjust off right wall
j_j205 8:290a110bcf0e 496 void Navigation::newLocalizeRight()
j_j205 8:290a110bcf0e 497 {
j_j205 8:290a110bcf0e 498 float k = 2.0;
j_j205 8:290a110bcf0e 499
j_j205 8:290a110bcf0e 500 float angleAdjust = k*(distLocalL - 13.0);
j_j205 8:290a110bcf0e 501
j_j205 8:290a110bcf0e 502 drive.move(0, angleAdjust*(PI / 180.0));
j_j205 7:f7489797746b 503 // wait for move to complete
j_j205 7:f7489797746b 504 while(!drive.isMoveDone())
j_j205 7:f7489797746b 505 {
j_j205 7:f7489797746b 506 wait(1e-6);
j_j205 7:f7489797746b 507 }
j_j205 8:290a110bcf0e 508 //angle += (-1*angleAdjust);
j_j205 8:290a110bcf0e 509 pc.printf("\n Adjusted angle by %f in newLocalizeRight", -1*angleAdjust);
j_j205 8:290a110bcf0e 510 }
j_j205 8:290a110bcf0e 511
j_j205 8:290a110bcf0e 512 // FUNCTION:
j_j205 8:290a110bcf0e 513 // void newLocalizeLeft()
j_j205 8:290a110bcf0e 514 // IN-PARAMETERS:
j_j205 8:290a110bcf0e 515 // None
j_j205 8:290a110bcf0e 516 // OUT-PARAMETERS:
j_j205 8:290a110bcf0e 517 // None
j_j205 8:290a110bcf0e 518 // DESCRIPTION:
j_j205 8:290a110bcf0e 519 // Right sensor to adjust off Left wall
j_j205 8:290a110bcf0e 520 void Navigation::newLocalizeLeft()
j_j205 8:290a110bcf0e 521 {
j_j205 8:290a110bcf0e 522 float k = 3.5;
j_j205 8:290a110bcf0e 523 float angleAdjust = k*(9.1 - distLocalR);
j_j205 8:290a110bcf0e 524
j_j205 8:290a110bcf0e 525 drive.move(0, angleAdjust*(PI / 180.0));
j_j205 8:290a110bcf0e 526 // wait for move to complete
j_j205 8:290a110bcf0e 527 while(!drive.isMoveDone())
j_j205 8:290a110bcf0e 528 {
j_j205 8:290a110bcf0e 529 wait(1e-6);
j_j205 8:290a110bcf0e 530 }
j_j205 8:290a110bcf0e 531 //angle += (-1*angleAdjust);
j_j205 8:290a110bcf0e 532 pc.printf("\n Adjusted angle by %f in newLocalizeLeft", -1*angleAdjust);
j_j205 8:290a110bcf0e 533 }
j_j205 8:290a110bcf0e 534
j_j205 8:290a110bcf0e 535 // FUNCTION:
j_j205 8:290a110bcf0e 536 // void newLocalizeLeftReverse()
j_j205 8:290a110bcf0e 537 // IN-PARAMETERS:
j_j205 8:290a110bcf0e 538 // None
j_j205 8:290a110bcf0e 539 // OUT-PARAMETERS:
j_j205 8:290a110bcf0e 540 // None
j_j205 8:290a110bcf0e 541 // DESCRIPTION:
j_j205 8:290a110bcf0e 542 // Right sensor to adjust off Left wall
j_j205 8:290a110bcf0e 543 void Navigation::newLocalizeLeftReverse()
j_j205 8:290a110bcf0e 544 {
j_j205 8:290a110bcf0e 545 float k = 3.5;
j_j205 8:290a110bcf0e 546 float angleAdjust = k*(9.1 - distLocalR);
j_j205 8:290a110bcf0e 547
j_j205 9:e24b50c1741b 548 drive.move(0, -1*(angleAdjust*(PI / 180.0)));
j_j205 8:290a110bcf0e 549 // wait for move to complete
j_j205 8:290a110bcf0e 550 while(!drive.isMoveDone())
j_j205 8:290a110bcf0e 551 {
j_j205 8:290a110bcf0e 552 wait(1e-6);
j_j205 8:290a110bcf0e 553 }
j_j205 8:290a110bcf0e 554 //angle += (-1*angleAdjust);
j_j205 9:e24b50c1741b 555 pc.printf("\n Adjusted angle by %f in newLocalizeLeftReverse", angleAdjust);
j_j205 8:290a110bcf0e 556 }
j_j205 8:290a110bcf0e 557
j_j205 8:290a110bcf0e 558 // FUNCTION:
j_j205 8:290a110bcf0e 559 // void newLocalizeRightReverse()
j_j205 8:290a110bcf0e 560 // IN-PARAMETERS:
j_j205 8:290a110bcf0e 561 // None
j_j205 8:290a110bcf0e 562 // OUT-PARAMETERS:
j_j205 8:290a110bcf0e 563 // None
j_j205 8:290a110bcf0e 564 // DESCRIPTION:
j_j205 8:290a110bcf0e 565 // Left sensor to adjust off right wall
j_j205 8:290a110bcf0e 566 void Navigation::newLocalizeRightReverse()
j_j205 8:290a110bcf0e 567 {
j_j205 8:290a110bcf0e 568 float k = 2.0;
j_j205 8:290a110bcf0e 569
j_j205 8:290a110bcf0e 570 float angleAdjust = k*(distLocalL - 13.0);
j_j205 8:290a110bcf0e 571
j_j205 9:e24b50c1741b 572 drive.move(0, -1*(angleAdjust*(PI / 180.0)));
j_j205 8:290a110bcf0e 573 // wait for move to complete
j_j205 8:290a110bcf0e 574 while(!drive.isMoveDone())
j_j205 8:290a110bcf0e 575 {
j_j205 8:290a110bcf0e 576 wait(1e-6);
j_j205 8:290a110bcf0e 577 }
j_j205 8:290a110bcf0e 578 //angle += (-1*angleAdjust);
j_j205 8:290a110bcf0e 579 pc.printf("\n Adjusted angle by %f in newLocalizeRight", angleAdjust);
j_j205 8:290a110bcf0e 580 }
j_j205 8:290a110bcf0e 581