Code to run the microcontrollers on the R5 competition bot
Dependencies: LineSensors mbed
Diff: navcontroller.cpp
- Revision:
- 19:9f4510646c9e
- Parent:
- 17:5046b27f5441
- Child:
- 22:8f726dc175cd
diff -r 2bd595af51d2 -r 9f4510646c9e navcontroller.cpp --- a/navcontroller.cpp Sat Apr 18 03:13:27 2015 +0000 +++ b/navcontroller.cpp Sat Apr 18 03:50:32 2015 +0000 @@ -6,17 +6,9 @@ #include <iostream> #include <vector> #include "navcontroller.h" -#include <opencv2/highgui/highgui.hpp> -#include <opencv2/imgproc/imgproc.hpp> -#include <tesseract/baseapi.h> -#include <unistd.h> -extern "C" { -#include <wiringX.h> -} NavController::NavController(int mapSize) : map(mapSize), orient(NORTH) { - initIO(); end.x = mapSize-1; end.y = mapSize-1; } @@ -50,13 +42,9 @@ saveMap(); } -void NavController::goBack() +void NavController::prepReturn() { - direction next; - typename ::position newPos; - map.setPosition(map.getStart()); - - stack<direction> temp; + stack<R5Map::direction> temp; while(!path.empty()) { @@ -65,41 +53,42 @@ } path = temp; - - - while(map.getPosition() != end) - { - next = path.top(); - path.pop(); - sendMove(next); - newPos = map.getPosition(); - - switch(next) - { - case NORTH : newPos.y++; - break; - case SOUTH : newPos.y--; - break; - case EAST : newPos.x++; - break; - case WEST : newPos.x--; - } - - map.setPosition(newPos); - } } - direction NavController::nextExploreMove() +R5Map::direction NavController::nextTraceMove() { - typename ::position current = map.getPosition(); - typename ::position dest = map.getFinish(); + R5Map::position newPos = map.getPosition(); + R5Map::nextMove; + + nextMove = path.top(); + path.pop(); + + switch(nextMove) + { + case NORTH : newPos.y++; + break; + case SOUTH : newPos.y--; + break; + case EAST : newPos.x++; + break; + case WEST : newPos.x--; + } + + map.setPosition(newPos); + return nextMove; +} + +R5Map::direction NavController::nextExploreMove() +{ + R5Map::position current = map.getPosition(); + R5Map::position dest = map.getFinish(); int northWgt = dest.y - current.y; int southWgt = current.y; int eastWgt = current.x; int westWgt = current.x - dest.x; - direction nextMove; + R5Map::direction nextMove; if(map.getVisited(NORTH) || map.getCurWall(NORTH) == CLOSED) northWgt = 0; @@ -115,7 +104,7 @@ if(northWgt != 0 || westWgt != 0) { - if(northWgt > westWgt) + if(northWgt > westWgt) nextMove = NORTH; else nextMove = WEST; @@ -133,7 +122,7 @@ } else { - direction lastMove = path.top(); + R5Map::direction lastMove = path.top(); path.pop(); switch(lastMove) @@ -151,33 +140,6 @@ return nextMove; } -void NavController::sendMove(direction next) -{ - int diff=next-orient; - - unsigned char letter; - - switch(diff) - { - case 0 : - letter='F'; - wiringXI2CWrite(frdm_i2c, letter);//forward - break; - case 1 : - letter='R'; - wiringXI2CWrite(frdm_i2c, letter);//right - break; - case 2 : - letter='B'; - wiringXI2CWrite(frdm_i2c, letter);//back - break; - case 3 : - letter='L'; - wiringXI2CWrite(frdm_i2c, letter);//left - } - cout << strerror(errno); -} - void NavController::updateMap() { if(!map.getCurVisited()) @@ -186,61 +148,15 @@ readSensor(SOUTH) ? map.setCurWall(SOUTH, CLOSED) : map.setCurWall(SOUTH, OPEN); readSensor(EAST) ? map.setCurWall(EAST, CLOSED) : map.setCurWall(EAST, OPEN); readSensor(WEST) ? map.setCurWall(WEST, CLOSED) : map.setCurWall(WEST, OPEN); - - vector<char> symbols = readWalls(); - - for(int i = 1; i <= symbols.size(); i++) - map.addCurSymbol(symbols.at(i)); } } -void NavController::saveMap() +bool NavController::readSensor(R5Map::direction wall) { - system("mkdir /media/flash; chmod 770 /media/flash; mount -t vfat /dev/sda /media/flash"); - char fileOut[100], - var[5]; - vector<char> symbols; - position spot; + int diff = wall - orient; - for(int i = 0; i < 7; i++) + switch(diff) { - for(int j = 0; j < 7; j++) - { - spot.x = i; - spot.y = j; - symbols = map.getSymbols(spot); - - if(!symbols.empty()) - { - strcat(fileOut, "cd /media/flash; cat > characters.txt; Cell ("); - sprintf(var, "%i", i); - strcat(fileOut, var); - strcat(fileOut, ","); - sprintf(var, "%i", j); - strcat(fileOut, var); - strcat(fileOut, ") : "); - strcat(fileOut, &symbols[0]); - - for(int k = 1; k < symbols.size(); k++) - { - strcat(fileOut, ", "); - strcat(fileOut, &symbols[k]); - } - } - - strcat(fileOut, "/n"); - system(fileOut); - } - } - - system("rmdir /media/flash; umount /dev/sda"); -} - -bool NavController::readSensor( direction wall){ - int diff; - diff=wall-orient; - - switch(diff){ case 0: digitalRead(SENS_FORWARD); case 1: @@ -250,162 +166,4 @@ case 3: digitalRead(SENS_LEFT); } -} - -char NavController::readWall( direction wall) -{ - turnCam(wall); - - - cv::VideoCapture cap(0); - if(!cap.isOpened()) //check if we succeeded - { - std::cerr << "ERROR: NavController::readWall( direction) failed to locate or open camera." << endl; - return -1; - } - - cv::Mat snap, gray; - cap >> snap; // get a new frame from camera - cvtColor(snap, gray, CV_BGR2GRAY); - // ...other image pre-processing here... - // Pass it to Tesseract API - tesseract::TessBaseAPI tess; - tess.Init(NULL, "eng", tesseract::OEM_DEFAULT); - tess.SetPageSegMode(tesseract::PSM_SINGLE_BLOCK); - tess.SetImage((uchar*)gray.data, gray.cols, gray.rows, 1, gray.cols); - - // Get the text - char* outPtr = tess.GetUTF8Text(); - char out = *outPtr; - - delete [] outPtr; - - return out; -} - -vector<char> NavController::readWalls() -{ - vector<char> out; - - if(map.getCurWall(NORTH) == CLOSED) - out.push_back(readWall(NORTH)); - if(map.getCurWall(SOUTH) == CLOSED) - out.push_back(readWall(SOUTH)); - if(map.getCurWall(EAST) == CLOSED) - out.push_back(readWall(EAST)); - if(map.getCurWall(WEST) == CLOSED) - out.push_back(readWall(WEST)); - - resetCam(); - - return out; -} - -void NavController::initIO(){ - if (wiringXSetup() < 0) - { - cout << "failed to initialize wiringX"; - exit (1) ; - } - wiringXISR(1,2); - //pin initializations go here - pinMode(6, OUTPUT); - pinMode(SENS_FORWARD, INPUT); - pinMode(SENS_RIGHT, INPUT); - pinMode(SENS_BACK, INPUT); - pinMode(SENS_LEFT, INPUT); - pinMode(STEP, OUTPUT); - pinMode(STEP_DIR, OUTPUT); - if((frdm_i2c=wiringXI2CSetup(FRDM_ADD))<0){ - cerr << "ERROR: Failed to establish I2C connection"<<endl; - } -} - -void NavController::turnCam( typename ::direction wall){ - int diff; - diff=wall-camorient; - - if(abs(diff)==3){ - diff/=-3; - } - - if(diff<0){ - digitalWrite(STEP_DIR, HIGH); - diff=abs(diff); - } - else{ - digitalWrite(STEP_DIR, LOW); - } - - diff*=1024; - - for(int i=0; i<diff; i++){ - digitalWrite(STEP, HIGH); - delayMicroseconds(5000); //.5 millisecond - digitalWrite(STEP, HIGH); - delayMicroseconds(5000); - } -} - -void NavController::resetCam(){ - int diff; - diff=orient-camorient; - - if(abs(diff)==3){ - diff/=-3; - } - - if(diff<0){ - digitalWrite(STEP_DIR, HIGH); - diff=abs(diff); - } - else{ - digitalWrite(STEP_DIR, LOW); - } - - diff*=1024; - - for(int i=0; i<diff; i++){ - digitalWrite(STEP, HIGH); - delayMicroseconds(5000); //millisecond - digitalWrite(STEP, HIGH); - delayMicroseconds(5000); - } -} - -void NavController::pinWrite(int pin, int lvl){ - if(lvl == 1) - digitalWrite(pin, HIGH); - else - digitalWrite(pin, LOW); -} - -void NavController::waitingForInterrupt(int pin, int set){ - waitForInterrupt(pin, set); -} - -void NavController::testWithDelay() -{ - while(true) - { - sendMove(NORTH); - usleep(500); - sendMove(WEST); - usleep(500); - sendMove(SOUTH); - usleep(500); - sendMove(EAST); - usleep(500); - } -} - -void NavController::testWithoutDelay() -{ - while(true) - { - sendMove(NORTH); - sendMove(WEST); - sendMove(SOUTH); - sendMove(EAST); - } -} +} \ No newline at end of file