Diff: navcontroller.cpp

Revision:

15:150ec9448efc

diff -r 7a4cf2ed2499 -r 150ec9448efc navcontroller.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/navcontroller.cpp	Sat Apr 18 02:54:55 2015 +0000
@@ -0,0 +1,403 @@
+#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);
+	}
+}