Texas State IEEE / Mbed 2 deprecated R5

Code to run the microcontrollers on the R5 competition bot

Dependencies:   LineSensors mbed

navcontroller.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?

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 #include <opencv2/highgui/highgui.hpp>
jmar11 17:5046b27f5441 10 #include <opencv2/imgproc/imgproc.hpp>
jmar11 17:5046b27f5441 11 #include <tesseract/baseapi.h>
jmar11 17:5046b27f5441 12 #include <unistd.h>
jmar11 17:5046b27f5441 13 extern "C" {
jmar11 17:5046b27f5441 14 #include <wiringX.h>
jmar11 17:5046b27f5441 15 }
jmar11 17:5046b27f5441 16
jmar11 17:5046b27f5441 17 NavController::NavController(int mapSize) : map(mapSize), orient(NORTH)
jmar11 17:5046b27f5441 18 {
jmar11 17:5046b27f5441 19 initIO();
jmar11 17:5046b27f5441 20 end.x = mapSize-1;
jmar11 17:5046b27f5441 21 end.y = mapSize-1;
jmar11 17:5046b27f5441 22 }
jmar11 17:5046b27f5441 23
jmar11 17:5046b27f5441 24 void NavController::explore()
jmar11 17:5046b27f5441 25 {
jmar11 17:5046b27f5441 26 typename ::direction next;
jmar11 17:5046b27f5441 27 typename ::position newPos;
jmar11 17:5046b27f5441 28
jmar11 17:5046b27f5441 29 while(map.getPosition() != end)
jmar11 17:5046b27f5441 30 {
jmar11 17:5046b27f5441 31 updateMap();
jmar11 17:5046b27f5441 32 next = nextExploreMove();
jmar11 17:5046b27f5441 33 sendMove(next);
jmar11 17:5046b27f5441 34 newPos = map.getPosition();
jmar11 17:5046b27f5441 35
jmar11 17:5046b27f5441 36 switch(next)
jmar11 17:5046b27f5441 37 {
jmar11 17:5046b27f5441 38 case NORTH : newPos.y++;
jmar11 17:5046b27f5441 39 break;
jmar11 17:5046b27f5441 40 case SOUTH : newPos.y--;
jmar11 17:5046b27f5441 41 break;
jmar11 17:5046b27f5441 42 case EAST : newPos.x++;
jmar11 17:5046b27f5441 43 break;
jmar11 17:5046b27f5441 44 case WEST : newPos.x--;
jmar11 17:5046b27f5441 45 }
jmar11 17:5046b27f5441 46
jmar11 17:5046b27f5441 47 map.setPosition(newPos);
jmar11 17:5046b27f5441 48 }
jmar11 17:5046b27f5441 49
jmar11 17:5046b27f5441 50 saveMap();
jmar11 17:5046b27f5441 51 }
jmar11 17:5046b27f5441 52
jmar11 17:5046b27f5441 53 void NavController::goBack()
jmar11 17:5046b27f5441 54 {
jmar11 17:5046b27f5441 55 direction next;
jmar11 17:5046b27f5441 56 typename ::position newPos;
jmar11 17:5046b27f5441 57 map.setPosition(map.getStart());
jmar11 17:5046b27f5441 58
jmar11 17:5046b27f5441 59 stack<direction> temp;
jmar11 17:5046b27f5441 60
jmar11 17:5046b27f5441 61 while(!path.empty())
jmar11 17:5046b27f5441 62 {
jmar11 17:5046b27f5441 63 temp.push(path.top());
jmar11 17:5046b27f5441 64 path.pop();
jmar11 17:5046b27f5441 65 }
jmar11 17:5046b27f5441 66
jmar11 17:5046b27f5441 67 path = temp;
jmar11 17:5046b27f5441 68
jmar11 17:5046b27f5441 69
jmar11 17:5046b27f5441 70 while(map.getPosition() != end)
jmar11 17:5046b27f5441 71 {
jmar11 17:5046b27f5441 72 next = path.top();
jmar11 17:5046b27f5441 73 path.pop();
jmar11 17:5046b27f5441 74 sendMove(next);
jmar11 17:5046b27f5441 75 newPos = map.getPosition();
jmar11 17:5046b27f5441 76
jmar11 17:5046b27f5441 77 switch(next)
jmar11 17:5046b27f5441 78 {
jmar11 17:5046b27f5441 79 case NORTH : newPos.y++;
jmar11 17:5046b27f5441 80 break;
jmar11 17:5046b27f5441 81 case SOUTH : newPos.y--;
jmar11 17:5046b27f5441 82 break;
jmar11 17:5046b27f5441 83 case EAST : newPos.x++;
jmar11 17:5046b27f5441 84 break;
jmar11 17:5046b27f5441 85 case WEST : newPos.x--;
jmar11 17:5046b27f5441 86 }
jmar11 17:5046b27f5441 87
jmar11 17:5046b27f5441 88 map.setPosition(newPos);
jmar11 17:5046b27f5441 89 }
jmar11 17:5046b27f5441 90 }
jmar11 17:5046b27f5441 91
jmar11 17:5046b27f5441 92 direction NavController::nextExploreMove()
jmar11 17:5046b27f5441 93 {
jmar11 17:5046b27f5441 94 typename ::position current = map.getPosition();
jmar11 17:5046b27f5441 95 typename ::position dest = map.getFinish();
jmar11 17:5046b27f5441 96
jmar11 17:5046b27f5441 97 int northWgt = dest.y - current.y;
jmar11 17:5046b27f5441 98 int southWgt = current.y;
jmar11 17:5046b27f5441 99 int eastWgt = current.x;
jmar11 17:5046b27f5441 100 int westWgt = current.x - dest.x;
jmar11 17:5046b27f5441 101
jmar11 17:5046b27f5441 102 direction nextMove;
jmar11 17:5046b27f5441 103
jmar11 17:5046b27f5441 104 if(map.getVisited(NORTH) || map.getCurWall(NORTH) == CLOSED)
jmar11 17:5046b27f5441 105 northWgt = 0;
jmar11 17:5046b27f5441 106
jmar11 17:5046b27f5441 107 if(map.getVisited(SOUTH) || map.getCurWall(SOUTH) == CLOSED)
jmar11 17:5046b27f5441 108 southWgt = 0;
jmar11 17:5046b27f5441 109
jmar11 17:5046b27f5441 110 if(map.getVisited(EAST) || map.getCurWall(EAST) == CLOSED)
jmar11 17:5046b27f5441 111 eastWgt = 0;
jmar11 17:5046b27f5441 112
jmar11 17:5046b27f5441 113 if(map.getVisited(WEST) || map.getCurWall(WEST) == CLOSED)
jmar11 17:5046b27f5441 114 westWgt = 0;
jmar11 17:5046b27f5441 115
jmar11 17:5046b27f5441 116 if(northWgt != 0 || westWgt != 0)
jmar11 17:5046b27f5441 117 {
jmar11 17:5046b27f5441 118 if(northWgt > westWgt)
jmar11 17:5046b27f5441 119 nextMove = NORTH;
jmar11 17:5046b27f5441 120 else
jmar11 17:5046b27f5441 121 nextMove = WEST;
jmar11 17:5046b27f5441 122
jmar11 17:5046b27f5441 123 path.push(nextMove);
jmar11 17:5046b27f5441 124 }
jmar11 17:5046b27f5441 125 else if(southWgt != 0 || eastWgt != 0)
jmar11 17:5046b27f5441 126 {
jmar11 17:5046b27f5441 127 if(southWgt > eastWgt)
jmar11 17:5046b27f5441 128 nextMove = SOUTH;
jmar11 17:5046b27f5441 129 else
jmar11 17:5046b27f5441 130 nextMove = EAST;
jmar11 17:5046b27f5441 131
jmar11 17:5046b27f5441 132 path.push(nextMove);
jmar11 17:5046b27f5441 133 }
jmar11 17:5046b27f5441 134 else
jmar11 17:5046b27f5441 135 {
jmar11 17:5046b27f5441 136 direction lastMove = path.top();
jmar11 17:5046b27f5441 137 path.pop();
jmar11 17:5046b27f5441 138
jmar11 17:5046b27f5441 139 switch(lastMove)
jmar11 17:5046b27f5441 140 {
jmar11 17:5046b27f5441 141 case NORTH : nextMove = SOUTH;
jmar11 17:5046b27f5441 142 break;
jmar11 17:5046b27f5441 143 case SOUTH : nextMove = NORTH;
jmar11 17:5046b27f5441 144 break;
jmar11 17:5046b27f5441 145 case EAST : nextMove = WEST;
jmar11 17:5046b27f5441 146 break;
jmar11 17:5046b27f5441 147 case WEST : nextMove = EAST;
jmar11 17:5046b27f5441 148 }
jmar11 17:5046b27f5441 149 }
jmar11 17:5046b27f5441 150
jmar11 17:5046b27f5441 151 return nextMove;
jmar11 17:5046b27f5441 152 }
jmar11 17:5046b27f5441 153
jmar11 17:5046b27f5441 154 void NavController::sendMove(direction next)
jmar11 17:5046b27f5441 155 {
jmar11 17:5046b27f5441 156 int diff=next-orient;
jmar11 17:5046b27f5441 157
jmar11 17:5046b27f5441 158 unsigned char letter;
jmar11 17:5046b27f5441 159
jmar11 17:5046b27f5441 160 switch(diff)
jmar11 17:5046b27f5441 161 {
jmar11 17:5046b27f5441 162 case 0 :
jmar11 17:5046b27f5441 163 letter='F';
jmar11 17:5046b27f5441 164 wiringXI2CWrite(frdm_i2c, letter);//forward
jmar11 17:5046b27f5441 165 break;
jmar11 17:5046b27f5441 166 case 1 :
jmar11 17:5046b27f5441 167 letter='R';
jmar11 17:5046b27f5441 168 wiringXI2CWrite(frdm_i2c, letter);//right
jmar11 17:5046b27f5441 169 break;
jmar11 17:5046b27f5441 170 case 2 :
jmar11 17:5046b27f5441 171 letter='B';
jmar11 17:5046b27f5441 172 wiringXI2CWrite(frdm_i2c, letter);//back
jmar11 17:5046b27f5441 173 break;
jmar11 17:5046b27f5441 174 case 3 :
jmar11 17:5046b27f5441 175 letter='L';
jmar11 17:5046b27f5441 176 wiringXI2CWrite(frdm_i2c, letter);//left
jmar11 17:5046b27f5441 177 }
jmar11 17:5046b27f5441 178 cout << strerror(errno);
jmar11 17:5046b27f5441 179 }
jmar11 17:5046b27f5441 180
jmar11 17:5046b27f5441 181 void NavController::updateMap()
jmar11 17:5046b27f5441 182 {
jmar11 17:5046b27f5441 183 if(!map.getCurVisited())
jmar11 17:5046b27f5441 184 {
jmar11 17:5046b27f5441 185 readSensor(NORTH) ? map.setCurWall(NORTH, CLOSED) : map.setCurWall(NORTH, OPEN);
jmar11 17:5046b27f5441 186 readSensor(SOUTH) ? map.setCurWall(SOUTH, CLOSED) : map.setCurWall(SOUTH, OPEN);
jmar11 17:5046b27f5441 187 readSensor(EAST) ? map.setCurWall(EAST, CLOSED) : map.setCurWall(EAST, OPEN);
jmar11 17:5046b27f5441 188 readSensor(WEST) ? map.setCurWall(WEST, CLOSED) : map.setCurWall(WEST, OPEN);
jmar11 17:5046b27f5441 189
jmar11 17:5046b27f5441 190 vector<char> symbols = readWalls();
jmar11 17:5046b27f5441 191
jmar11 17:5046b27f5441 192 for(int i = 1; i <= symbols.size(); i++)
jmar11 17:5046b27f5441 193 map.addCurSymbol(symbols.at(i));
jmar11 17:5046b27f5441 194 }
jmar11 17:5046b27f5441 195 }
jmar11 17:5046b27f5441 196
jmar11 17:5046b27f5441 197 void NavController::saveMap()
jmar11 17:5046b27f5441 198 {
jmar11 17:5046b27f5441 199 system("mkdir /media/flash; chmod 770 /media/flash; mount -t vfat /dev/sda /media/flash");
jmar11 17:5046b27f5441 200 char fileOut[100],
jmar11 17:5046b27f5441 201 var[5];
jmar11 17:5046b27f5441 202 vector<char> symbols;
jmar11 17:5046b27f5441 203 position spot;
jmar11 17:5046b27f5441 204
jmar11 17:5046b27f5441 205 for(int i = 0; i < 7; i++)
jmar11 17:5046b27f5441 206 {
jmar11 17:5046b27f5441 207 for(int j = 0; j < 7; j++)
jmar11 17:5046b27f5441 208 {
jmar11 17:5046b27f5441 209 spot.x = i;
jmar11 17:5046b27f5441 210 spot.y = j;
jmar11 17:5046b27f5441 211 symbols = map.getSymbols(spot);
jmar11 17:5046b27f5441 212
jmar11 17:5046b27f5441 213 if(!symbols.empty())
jmar11 17:5046b27f5441 214 {
jmar11 17:5046b27f5441 215 strcat(fileOut, "cd /media/flash; cat > characters.txt; Cell (");
jmar11 17:5046b27f5441 216 sprintf(var, "%i", i);
jmar11 17:5046b27f5441 217 strcat(fileOut, var);
jmar11 17:5046b27f5441 218 strcat(fileOut, ",");
jmar11 17:5046b27f5441 219 sprintf(var, "%i", j);
jmar11 17:5046b27f5441 220 strcat(fileOut, var);
jmar11 17:5046b27f5441 221 strcat(fileOut, ") : ");
jmar11 17:5046b27f5441 222 strcat(fileOut, &symbols[0]);
jmar11 17:5046b27f5441 223
jmar11 17:5046b27f5441 224 for(int k = 1; k < symbols.size(); k++)
jmar11 17:5046b27f5441 225 {
jmar11 17:5046b27f5441 226 strcat(fileOut, ", ");
jmar11 17:5046b27f5441 227 strcat(fileOut, &symbols[k]);
jmar11 17:5046b27f5441 228 }
jmar11 17:5046b27f5441 229 }
jmar11 17:5046b27f5441 230
jmar11 17:5046b27f5441 231 strcat(fileOut, "/n");
jmar11 17:5046b27f5441 232 system(fileOut);
jmar11 17:5046b27f5441 233 }
jmar11 17:5046b27f5441 234 }
jmar11 17:5046b27f5441 235
jmar11 17:5046b27f5441 236 system("rmdir /media/flash; umount /dev/sda");
jmar11 17:5046b27f5441 237 }
jmar11 17:5046b27f5441 238
jmar11 17:5046b27f5441 239 bool NavController::readSensor( direction wall){
jmar11 17:5046b27f5441 240 int diff;
jmar11 17:5046b27f5441 241 diff=wall-orient;
jmar11 17:5046b27f5441 242
jmar11 17:5046b27f5441 243 switch(diff){
jmar11 17:5046b27f5441 244 case 0:
jmar11 17:5046b27f5441 245 digitalRead(SENS_FORWARD);
jmar11 17:5046b27f5441 246 case 1:
jmar11 17:5046b27f5441 247 digitalRead(SENS_RIGHT);
jmar11 17:5046b27f5441 248 case 2:
jmar11 17:5046b27f5441 249 digitalRead(SENS_BACK);
jmar11 17:5046b27f5441 250 case 3:
jmar11 17:5046b27f5441 251 digitalRead(SENS_LEFT);
jmar11 17:5046b27f5441 252 }
jmar11 17:5046b27f5441 253 }
jmar11 17:5046b27f5441 254
jmar11 17:5046b27f5441 255 char NavController::readWall( direction wall)
jmar11 17:5046b27f5441 256 {
jmar11 17:5046b27f5441 257 turnCam(wall);
jmar11 17:5046b27f5441 258
jmar11 17:5046b27f5441 259
jmar11 17:5046b27f5441 260 cv::VideoCapture cap(0);
jmar11 17:5046b27f5441 261 if(!cap.isOpened()) //check if we succeeded
jmar11 17:5046b27f5441 262 {
jmar11 17:5046b27f5441 263 std::cerr << "ERROR: NavController::readWall( direction) failed to locate or open camera." << endl;
jmar11 17:5046b27f5441 264 return -1;
jmar11 17:5046b27f5441 265 }
jmar11 17:5046b27f5441 266
jmar11 17:5046b27f5441 267 cv::Mat snap, gray;
jmar11 17:5046b27f5441 268 cap >> snap; // get a new frame from camera
jmar11 17:5046b27f5441 269 cvtColor(snap, gray, CV_BGR2GRAY);
jmar11 17:5046b27f5441 270 // ...other image pre-processing here...
jmar11 17:5046b27f5441 271 // Pass it to Tesseract API
jmar11 17:5046b27f5441 272 tesseract::TessBaseAPI tess;
jmar11 17:5046b27f5441 273 tess.Init(NULL, "eng", tesseract::OEM_DEFAULT);
jmar11 17:5046b27f5441 274 tess.SetPageSegMode(tesseract::PSM_SINGLE_BLOCK);
jmar11 17:5046b27f5441 275 tess.SetImage((uchar*)gray.data, gray.cols, gray.rows, 1, gray.cols);
jmar11 17:5046b27f5441 276
jmar11 17:5046b27f5441 277 // Get the text
jmar11 17:5046b27f5441 278 char* outPtr = tess.GetUTF8Text();
jmar11 17:5046b27f5441 279 char out = *outPtr;
jmar11 17:5046b27f5441 280
jmar11 17:5046b27f5441 281 delete [] outPtr;
jmar11 17:5046b27f5441 282
jmar11 17:5046b27f5441 283 return out;
jmar11 17:5046b27f5441 284 }
jmar11 17:5046b27f5441 285
jmar11 17:5046b27f5441 286 vector<char> NavController::readWalls()
jmar11 17:5046b27f5441 287 {
jmar11 17:5046b27f5441 288 vector<char> out;
jmar11 17:5046b27f5441 289
jmar11 17:5046b27f5441 290 if(map.getCurWall(NORTH) == CLOSED)
jmar11 17:5046b27f5441 291 out.push_back(readWall(NORTH));
jmar11 17:5046b27f5441 292 if(map.getCurWall(SOUTH) == CLOSED)
jmar11 17:5046b27f5441 293 out.push_back(readWall(SOUTH));
jmar11 17:5046b27f5441 294 if(map.getCurWall(EAST) == CLOSED)
jmar11 17:5046b27f5441 295 out.push_back(readWall(EAST));
jmar11 17:5046b27f5441 296 if(map.getCurWall(WEST) == CLOSED)
jmar11 17:5046b27f5441 297 out.push_back(readWall(WEST));
jmar11 17:5046b27f5441 298
jmar11 17:5046b27f5441 299 resetCam();
jmar11 17:5046b27f5441 300
jmar11 17:5046b27f5441 301 return out;
jmar11 17:5046b27f5441 302 }
jmar11 17:5046b27f5441 303
jmar11 17:5046b27f5441 304 void NavController::initIO(){
jmar11 17:5046b27f5441 305 if (wiringXSetup() < 0)
jmar11 17:5046b27f5441 306 {
jmar11 17:5046b27f5441 307 cout << "failed to initialize wiringX";
jmar11 17:5046b27f5441 308 exit (1) ;
jmar11 17:5046b27f5441 309 }
jmar11 17:5046b27f5441 310 wiringXISR(1,2);
jmar11 17:5046b27f5441 311 //pin initializations go here
jmar11 17:5046b27f5441 312 pinMode(6, OUTPUT);
jmar11 17:5046b27f5441 313 pinMode(SENS_FORWARD, INPUT);
jmar11 17:5046b27f5441 314 pinMode(SENS_RIGHT, INPUT);
jmar11 17:5046b27f5441 315 pinMode(SENS_BACK, INPUT);
jmar11 17:5046b27f5441 316 pinMode(SENS_LEFT, INPUT);
jmar11 17:5046b27f5441 317 pinMode(STEP, OUTPUT);
jmar11 17:5046b27f5441 318 pinMode(STEP_DIR, OUTPUT);
jmar11 17:5046b27f5441 319 if((frdm_i2c=wiringXI2CSetup(FRDM_ADD))<0){
jmar11 17:5046b27f5441 320 cerr << "ERROR: Failed to establish I2C connection"<<endl;
jmar11 17:5046b27f5441 321 }
jmar11 17:5046b27f5441 322 }
jmar11 17:5046b27f5441 323
jmar11 17:5046b27f5441 324 void NavController::turnCam( typename ::direction wall){
jmar11 17:5046b27f5441 325 int diff;
jmar11 17:5046b27f5441 326 diff=wall-camorient;
jmar11 17:5046b27f5441 327
jmar11 17:5046b27f5441 328 if(abs(diff)==3){
jmar11 17:5046b27f5441 329 diff/=-3;
jmar11 17:5046b27f5441 330 }
jmar11 17:5046b27f5441 331
jmar11 17:5046b27f5441 332 if(diff<0){
jmar11 17:5046b27f5441 333 digitalWrite(STEP_DIR, HIGH);
jmar11 17:5046b27f5441 334 diff=abs(diff);
jmar11 17:5046b27f5441 335 }
jmar11 17:5046b27f5441 336 else{
jmar11 17:5046b27f5441 337 digitalWrite(STEP_DIR, LOW);
jmar11 17:5046b27f5441 338 }
jmar11 17:5046b27f5441 339
jmar11 17:5046b27f5441 340 diff*=1024;
jmar11 17:5046b27f5441 341
jmar11 17:5046b27f5441 342 for(int i=0; i<diff; i++){
jmar11 17:5046b27f5441 343 digitalWrite(STEP, HIGH);
jmar11 17:5046b27f5441 344 delayMicroseconds(5000); //.5 millisecond
jmar11 17:5046b27f5441 345 digitalWrite(STEP, HIGH);
jmar11 17:5046b27f5441 346 delayMicroseconds(5000);
jmar11 17:5046b27f5441 347 }
jmar11 17:5046b27f5441 348 }
jmar11 17:5046b27f5441 349
jmar11 17:5046b27f5441 350 void NavController::resetCam(){
jmar11 17:5046b27f5441 351 int diff;
jmar11 17:5046b27f5441 352 diff=orient-camorient;
jmar11 17:5046b27f5441 353
jmar11 17:5046b27f5441 354 if(abs(diff)==3){
jmar11 17:5046b27f5441 355 diff/=-3;
jmar11 17:5046b27f5441 356 }
jmar11 17:5046b27f5441 357
jmar11 17:5046b27f5441 358 if(diff<0){
jmar11 17:5046b27f5441 359 digitalWrite(STEP_DIR, HIGH);
jmar11 17:5046b27f5441 360 diff=abs(diff);
jmar11 17:5046b27f5441 361 }
jmar11 17:5046b27f5441 362 else{
jmar11 17:5046b27f5441 363 digitalWrite(STEP_DIR, LOW);
jmar11 17:5046b27f5441 364 }
jmar11 17:5046b27f5441 365
jmar11 17:5046b27f5441 366 diff*=1024;
jmar11 17:5046b27f5441 367
jmar11 17:5046b27f5441 368 for(int i=0; i<diff; i++){
jmar11 17:5046b27f5441 369 digitalWrite(STEP, HIGH);
jmar11 17:5046b27f5441 370 delayMicroseconds(5000); //millisecond
jmar11 17:5046b27f5441 371 digitalWrite(STEP, HIGH);
jmar11 17:5046b27f5441 372 delayMicroseconds(5000);
jmar11 17:5046b27f5441 373 }
jmar11 17:5046b27f5441 374 }
jmar11 17:5046b27f5441 375
jmar11 17:5046b27f5441 376 void NavController::pinWrite(int pin, int lvl){
jmar11 17:5046b27f5441 377 if(lvl == 1)
jmar11 17:5046b27f5441 378 digitalWrite(pin, HIGH);
jmar11 17:5046b27f5441 379 else
jmar11 17:5046b27f5441 380 digitalWrite(pin, LOW);
jmar11 17:5046b27f5441 381 }
jmar11 17:5046b27f5441 382
jmar11 17:5046b27f5441 383 void NavController::waitingForInterrupt(int pin, int set){
jmar11 17:5046b27f5441 384 waitForInterrupt(pin, set);
jmar11 17:5046b27f5441 385 }
jmar11 17:5046b27f5441 386
jmar11 17:5046b27f5441 387 void NavController::testWithDelay()
jmar11 17:5046b27f5441 388 {
jmar11 17:5046b27f5441 389 while(true)
jmar11 17:5046b27f5441 390 {
jmar11 17:5046b27f5441 391 sendMove(NORTH);
jmar11 17:5046b27f5441 392 usleep(500);
jmar11 17:5046b27f5441 393 sendMove(WEST);
jmar11 17:5046b27f5441 394 usleep(500);
jmar11 17:5046b27f5441 395 sendMove(SOUTH);
jmar11 17:5046b27f5441 396 usleep(500);
jmar11 17:5046b27f5441 397 sendMove(EAST);
jmar11 17:5046b27f5441 398 usleep(500);
jmar11 17:5046b27f5441 399 }
jmar11 17:5046b27f5441 400 }
jmar11 17:5046b27f5441 401
jmar11 17:5046b27f5441 402 void NavController::testWithoutDelay()
jmar11 17:5046b27f5441 403 {
jmar11 17:5046b27f5441 404 while(true)
jmar11 17:5046b27f5441 405 {
jmar11 17:5046b27f5441 406 sendMove(NORTH);
jmar11 17:5046b27f5441 407 sendMove(WEST);
jmar11 17:5046b27f5441 408 sendMove(SOUTH);
jmar11 17:5046b27f5441 409 sendMove(EAST);
jmar11 17:5046b27f5441 410 }
jmar11 17:5046b27f5441 411 }