Code to run the microcontrollers on the R5 competition bot
Dependencies: LineSensors mbed
Diff: navcontroller.cpp
- Revision:
- 17:5046b27f5441
- Child:
- 19:9f4510646c9e
diff -r b49db5c8e16c -r 5046b27f5441 navcontroller.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/navcontroller.cpp Sat Apr 18 03:02:28 2015 +0000 @@ -0,0 +1,411 @@ +#include <cstdlib> +#include <cstring> +#include <cstdio> +#include <sstream> +#include <string> +#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; +} + +void NavController::explore() +{ + typename ::direction next; + typename ::position newPos; + + while(map.getPosition() != end) + { + updateMap(); + next = nextExploreMove(); + 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); + } + + saveMap(); +} + +void NavController::goBack() +{ + direction next; + typename ::position newPos; + map.setPosition(map.getStart()); + + stack<direction> temp; + + while(!path.empty()) + { + temp.push(path.top()); + path.pop(); + } + + 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() +{ + typename ::position current = map.getPosition(); + typename ::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; + + if(map.getVisited(NORTH) || map.getCurWall(NORTH) == CLOSED) + northWgt = 0; + + if(map.getVisited(SOUTH) || map.getCurWall(SOUTH) == CLOSED) + southWgt = 0; + + if(map.getVisited(EAST) || map.getCurWall(EAST) == CLOSED) + eastWgt = 0; + + if(map.getVisited(WEST) || map.getCurWall(WEST) == CLOSED) + westWgt = 0; + + if(northWgt != 0 || westWgt != 0) + { + if(northWgt > westWgt) + nextMove = NORTH; + else + nextMove = WEST; + + path.push(nextMove); + } + else if(southWgt != 0 || eastWgt != 0) + { + if(southWgt > eastWgt) + nextMove = SOUTH; + else + nextMove = EAST; + + path.push(nextMove); + } + else + { + direction lastMove = path.top(); + path.pop(); + + switch(lastMove) + { + case NORTH : nextMove = SOUTH; + break; + case SOUTH : nextMove = NORTH; + break; + case EAST : nextMove = WEST; + break; + case WEST : nextMove = EAST; + } + } + + 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()) + { + readSensor(NORTH) ? map.setCurWall(NORTH, CLOSED) : map.setCurWall(NORTH, OPEN); + 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() +{ + 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; + + for(int i = 0; i < 7; i++) + { + 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: + digitalRead(SENS_RIGHT); + case 2: + digitalRead(SENS_BACK); + 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); + } +}