Texas State IEEE / Mbed 2 deprecated R5

Code to run the microcontrollers on the R5 competition bot

Dependencies:   LineSensors mbed

navcontroller.cpp@15:150ec9448efc, 2015-04-18 (annotated)

Committer:
Hypna
Date:
Sat Apr 18 02:54:55 2015 +0000
Revision:
15:150ec9448efc
experiment

Who changed what in which revision?

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