File content as of revision 15:150ec9448efc:

#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>
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;
	
	switch(diff)
	{
		case 0 :
			wiringXI2CWrite(frdm_i2c, 'F');//forward
			break;
		case 1 :
			wiringXI2CWrite(frdm_i2c, 'R');//right
			break;
		case 2 :
			wiringXI2CWrite(frdm_i2c, 'B');//back
			break;
		case 3 :
			wiringXI2CWrite(frdm_i2c, 'L');//left
	}
}

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