Diff: navcontroller.cpp

Revision:

17:5046b27f5441

Child:

19:9f4510646c9e

--- /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);
+    }
+}