File content as of revision 17:5046b27f5441:

#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);
    }
}