int main (int argc, char * const argv[]) {
char code;
bool draw = true;
Mat image;
image = Mat::zeros(361, 301, CV_8UC3);
image = cv::Scalar::all(0);
setup();
std::vector<Point > edges;
std::vector<Point > doors;
std::vector<Point > docks;
Point arcReactorLoc;
parseFile(edges,doors,docks,arcReactorLoc);
drawMap(image, edges, doors, docks, arcReactorLoc);
cv::Point pos(15, 15);
MapNode* nodes[6][5];
//generate nodes
for(int y = 0; y < 6; y++)
{
for(int x = 0; x < 5; x++)
{
cv::Point* nodeLoc = new cv::Point((x * 60) + 30, (y * 60) + 30);
MapNode* mapNode = new MapNode(*nodeLoc, x, y);
nodes[y][x] = mapNode;
}
}
//figure out where each node can go
for(int y = 0; y < 6; y++)
{
for(int x = 0; x < 5; x++)
{
//MapNode mapNode(nodeLoc);
MapNode* mapNode = nodes[y][x];
cv::Point nodeLoc = mapNode->getPosition();
if(y - 1 >= 0)
mapNode->setDown((nodes[y - 1][x]));
if(y + 1 <= 5)
mapNode->setUp((MapNode*)(nodes[y + 1][x]));
if(x - 1 >= 0)
mapNode->setLeft((nodes[y][x - 1]));
if(x + 1 <= 4)
mapNode->setRight((nodes[y][x + 1]));
for(int move = 0; move < 31; move++)
{
//check up
if((mapNode->getUp() != NULL) && (image.at<Vec3b>(nodeLoc.y + move, nodeLoc.x).val[0] == 255))
mapNode->setUp(NULL);
//check down
if(mapNode->getDown() != NULL && (image.at<Vec3b>(nodeLoc.y - move, nodeLoc.x).val[0] == 255))
mapNode->setDown(NULL);
//check left
if(mapNode->getLeft() != NULL && image.at<Vec3b>(nodeLoc.y, nodeLoc.x - move).val[0] == 255)
mapNode->setLeft(NULL);
//check right
if(mapNode->getRight() != NULL && image.at<Vec3b>(nodeLoc.y, nodeLoc.x + move).val[0] == 255)
mapNode->setRight(NULL);
}
}
}
for(int y = 0; y < 6; y++)
{
for(int x = 0; x < 5; x++)
{
MapNode* mapNode = nodes[y][x];
cv::Point nodeLoc = mapNode->getPosition();
if(mapNode->getDown() != NULL)
line(image, mapNode->getPosition(), mapNode->getDown()->getPosition(), Scalar(0, 255, 0), 4, 6);
if(mapNode->getUp() != NULL)
line(image, mapNode->getPosition(), (mapNode->getUp())->getPosition(), Scalar(0, 255, 0), 4, 6);
if(mapNode->getLeft() != NULL)
line(image, mapNode->getPosition(), mapNode->getLeft()->getPosition(), Scalar(0, 255, 0), 4, 6);
if(mapNode->getRight() != NULL)
line(image, mapNode->getPosition(), mapNode->getRight()->getPosition(), Scalar(0, 255, 0), 4, 6);
}
}
cv :: Mat logo = cv :: imread ("ironman_icon.jpg");
cv :: Mat imageROI;
imageROI = image (cv :: Rect (pos.x, pos.y, logo.cols, logo.rows));
logo.copyTo (imageROI);
cv :: namedWindow("result");
cv :: imshow ("result", image);
cv::waitKey(0);
std::vector<MapNode*> possibleNodes(30);
//std::vector<MapNode*> possibleReverseNodes(0);
int i = 0;
for(int y = 0; y < 6; y++)
{
for(int x = 0; x < 5; x++)
{
possibleNodes[i++] = nodes[y][x];
}
}
printf("init size of possible locactions %i\n", possibleNodes.size());
int lastMove = 0;
//list of all nodes made, check sonar and remove impossible nodes
//bool reverse = false;
while(possibleNodes.size() > 1)
{
move(UP, true);
int hasUp = checkSonar(1) / 60;
move(RIGHT, true);
int hasRight = checkSonar(1) / 60;
move(DOWN, true);
int hasDown = checkSonar(1) / 60;
move(LEFT, true);
int hasLeft = checkSonar(1) / 60;
//int before = possibleReverseNodes.size();
printf("size %i u%i d%i l%i r%i\n", possibleNodes.size(), hasDown, hasUp, hasLeft, hasRight);
for(int j = 0; j < possibleNodes.size(); j++)
{
//remove nodes that dont match this
if(possibleNodes[j]->getDownFull() != hasDown)
{
possibleNodes.erase(possibleNodes.begin() + j);
j--;
continue;
}
if(possibleNodes[j]->getUpFull() != hasUp)
{
possibleNodes.erase(possibleNodes.begin() + j);
j--;
continue;
}
if(hasRight && possibleNodes[j]->getRightFull() < hasRight)
{
/**if(reverse)
continue;
possibleReverseNodes.push_back(possibleNodes[j]);**/
possibleNodes.erase(possibleNodes.begin() + j);
j--;
continue;
}
if(hasLeft && possibleNodes[j]->getLeftFull() < hasLeft)
{
/**if(reverse)
continue;
possibleReverseNodes.push_back(possibleNodes[j]);**/
possibleNodes.erase(possibleNodes.begin() + j);
j--;
continue;
}
}
cout << possibleNodes.size() << endl;
if(possibleNodes.size() == 0)
{
printf("error\n");
printf("error\n");
printf("error\n");
printf("error\n");
return 0;
}
/**else if(possibleNodes.size() == 0)
{
/**for(int toAdd = 0; possibleReverseNodes.size(); toAdd++)
{
possibleNodes.push_back(possibleReverseNodes.pop_back());
}
possibleNodes.reserve(possibleNodes.size() + possibleReverseNodes.size());
copy(possibleReverseNodes.begin(), possibleReverseNodes.end(), inserter(possibleNodes, possibleNodes.end()));
possibleReverseNodes.clear();
reverse = true;
}
else
{
reverse = false;
}**/
//do we need to move again
if(possibleNodes.size() > 1)
{
//printf("ran\n");
//favor up 1 and right 2
repeat_this_code:
if(hasUp && (lastMove & 4) != 4)
{
printf("went up\n");
move(UP);
lastMove |= 1;
for(int z=0; z < possibleNodes.size(); z++)
{
possibleNodes[z] = possibleNodes[z]->getUp();
}
}
else if(hasRight && (lastMove & 8) != 8)
{
printf("went right\n");
lastMove |= 2;
move(RIGHT);
for(int z=0; z < possibleNodes.size(); z++)
{
possibleNodes[z] = possibleNodes[z]->getRight();
}
}
else if(hasDown && (lastMove & 1) != 1)
{
lastMove |= 4;
printf("went down\n");
move(DOWN);
for(int z=0; z < possibleNodes.size(); z++)
{
possibleNodes[z] = possibleNodes[z]->getDown();
}
}
else if(hasLeft && (lastMove & 2) != 2)
{
lastMove |= 8;
printf("went left\n");
move(LEFT);
for(int z=0; z < possibleNodes.size(); z++)
{
possibleNodes[z] = possibleNodes[z]->getLeft();
}
}
else
{
if(hasLeft && (lastMove & 8) != 8)
{
lastMove &= ~2;
lastMove |= 8;
move(LEFT);
}
else if(hasDown && (lastMove & 4) != 4)
{
lastMove &= ~1;
lastMove |= 4;
move(DOWN);
}
else
{
lastMove = 0;
goto repeat_this_code;
}
}
}
}
cout << "found?\n";
MapNode* node = possibleNodes[0];
printf("found robot at %i %i\n", node->getPosX(), node->getPosY());
//should know where we are by this point
redrawMacro();
//**********************
//OPEN UP VIDEO STUFF
//**********************
VideoCapture cap(0); //capture the video from webcam
if ( !cap.isOpened() ) // if not success, exit program
{
cout << "Cannot open the web cam" << endl;
return -1;
}
cap.set(CV_CAP_PROP_FRAME_WIDTH, 160);
cap.set(CV_CAP_PROP_FRAME_HEIGHT, 120);
// namedWindow("Control", CV_WINDOW_AUTOSIZE); //create a window called "Control"
//Capture a temporary image from the camera
Mat imgTmp;
imgTmp = Mat::zeros(301, 301, CV_8UC3);
cap.read(imgTmp);
//Create a black image with the size as the camera output
Mat imgLines = Mat::zeros( imgTmp.size(), CV_8UC3 );;
//********************
//end camera open
//********************
//start doing tasks
int positionCheck = 0;
bool visitedA = false;
bool visitedB = false;
bool visited2 = false;
while(!visitedA || !visitedB)
{
MapNode* toVisit = NULL;
switch(positionCheck)
{
case 0:
toVisit = nodes[0][2];
break;
case 1:
toVisit = nodes[0][3];
break;
case 2:
toVisit = nodes[3][0];
break;
case 3:
cout << "Visited all locations, missed a lab somewhere probably, error and ending\n";
return 0;
}
if(positionCheck == 1)
{
string tempP = pathFind(node->getX(), node->getY(), toVisit->getX(), toVisit->getY(), nodes);
toVisit = nodes[3][0];
if(tempP.length() > pathFind(node->getX(), node->getY(), toVisit->getX(), toVisit->getY(), nodes).length())
visited2 = true;
else
toVisit = nodes[0][3];
}
else if(positionCheck == 2)
if(!visited2)
toVisit = nodes[3][0];
else
toVisit = nodes[0][3];
//move to node we are visiting
node = node->traverse(toVisit, pathFind(node->getX(), node->getY(), toVisit->getX(), toVisit->getY(), nodes));
redrawMacro();
positionCheck++; //setup for next run if necessary
//check sonar
int toWallDist = checkSonar(1);
if(toWallDist < 20)
{
while(checkSonar(1) < 20)
{
setSpeed(fp, ZERO - (180 - ZERO), 180);
delay(20);
setSpeed(fp, 0, 0);
}
}
//camera check
bool found = false;
bool foundA = false;
bool lookLeft = false;
bool lookRight = false;
bool lookStr = false;
for(int tries = 0; tries < CAMERA_TRIES_MAX; tries++)
{
Mat imgOriginal;
bool bSuccess = cap.read(imgOriginal); // read a new frame from video
if (!bSuccess) //if not success, break loop
{
cout << "Cannot read a frame from video stream" << endl;
return 0;
}
Mat imgHSV;
cvtColor(imgOriginal, imgHSV, COLOR_BGR2HSV); //Convert the captured frame from BGR to HSV
if(!visitedA)
{
Mat imgThresholded;
//booklet
int iLowH = 80;
int iHighH = 102;
int iLowS = 32;
int iHighS = 255;
int iLowV = 91;
int iHighV = 255;
inRange(imgHSV, Scalar(iLowH, iLowS, iLowV), Scalar(iHighH, iHighS, iHighV), imgThresholded); //Threshold the image
//morphological opening (removes small objects from the foreground)
erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) );
dilate( imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) );
//morphological closing (removes small holes from the foreground)
dilate( imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) );
erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) );
//Calculate the moments of the thresholded image
Moments oMoments = moments(imgThresholded);
double dM01 = oMoments.m01;
double dM10 = oMoments.m10;
double dArea = oMoments.m00;
found = dArea > IMAGE_THRES_REQ;
if(found)
{
foundA = true;
break;
}
}
if(!visitedB && !found)
{
Mat imgThresholded;
//green lego
int iLowH = 63;
int iHighH = 77;
int iLowS = 73;
int iHighS = 255;
int iLowV = 60;
int iHighV = 255;
inRange(imgHSV, Scalar(iLowH, iLowS, iLowV), Scalar(iHighH, iHighS, iHighV), imgThresholded); //Threshold the image
//morphological opening (removes small objects from the foreground)
erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) );
dilate( imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) );
//morphological closing (removes small holes from the foreground)
dilate( imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) );
erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) );
//Calculate the moments of the thresholded image
Moments oMoments = moments(imgThresholded);
double dM01 = oMoments.m01;
double dM10 = oMoments.m10;
double dArea = oMoments.m00;
found = dArea > IMAGE_THRES_REQ;
if(found)
break;
}
if(!lookLeft)
{
lookLeft = true;
setSpeed(fp, 180, 180);
delay(200);
setSpeed(fp, 0, 0);
}
else if(!lookRight)
{
lookRight = true;
setSpeed(fp, ZERO - (180 - ZERO), ZERO - (180 - ZERO));
delay(400);
setSpeed(fp, 0, 0);
}
else if(!lookStr)
{
lookStr = true;
setSpeed(fp, 180, 180);
delay(200);
setSpeed(fp, 0, 0);
}
}
if(found && foundA)
{
cout << "Found virus A, going to lab A\n";
node = node->traverse(nodes[4][0], pathFind(node->getX(), node->getY(), 0, 4, nodes));
redrawMacro();
visitedA = true;
}
else if(found)
{
cout << "Found virus B, going to lab B\n";
node = node->traverse(nodes[4][4], pathFind(node->getX(), node->getY(), 4, 4, nodes));
redrawMacro();
visitedB = true;
}
else
{
cout << "No virus found, proceeding to next location!\n";
}
//if we looked left without looking straight, we need to fix position
if(lookLeft && !lookStr)
{
lookRight = true;
setSpeed(fp, ZERO - (180 - ZERO), ZERO - (180 - ZERO));
delay(100);
setSpeed(fp, 0, 0);
}
}
printf("robot should be done\n");
return 0;
}
void redraw(cv::Mat logo, Point pos, cv::Mat image, std::vector<Point> &edges, std::vector<Point> &virus,
std::vector<Point> &labA,
Point &labB, MapNode* nodes[][5])
{
static cv::Mat clear = Mat::zeros(361, 301, CV_8UC3);
clear.copyTo(image);
int thickness = 6;
int lineType = 8;
//Draw Wall
for(int i = 0;i < edges.size()-1; i+=2){
Point p1 = edges[i];
Point p2 = edges[i+1];
line (image, p1, p2, Scalar(255, 0, 0), thickness, lineType);
}
//Draw Virus Location
for(int i = 0;i < virus.size()-1; i+=2) {
Point p1 = virus[i];
Point p2 = virus[i+1];
Point topLeft1, topRight1, bottomLeft1, bottomRight1;
topLeft1.x = int (p1.x - p2.x/2);
topLeft1.y = int (p1.y - p2.y/2);
topRight1.x = int (p1.x + p2.x/2);
topRight1.y = int (p1.y - p2.y/2);
bottomLeft1.x = int (p1.x - p2.x/2);
bottomLeft1.y = int (p1.y + p2.y/2);
bottomRight1.x = int (p1.x + p2.x/2);
bottomRight1.y = int (p1.y + p2.y/2);
line(image,topLeft1, bottomRight1, CV_RGB(0, 150, 150));
line(image,topRight1, bottomLeft1, CV_RGB(0, 150, 150));
}
//Draw LabA Location
for(int i = 0;i < labA.size()-1; i+=2){
Point p1 = labA[i];
Point p2 = labA[i+1];
Point topLeft, topRight, bottomLeft, bottomRight;
topLeft.x = int (p1.x - p2.x/2);
topLeft.y = int (p1.y - p2.y/2);
topRight.x = int (p1.x + p2.x/2);
topRight.y = int (p1.y - p2.y/2);
bottomLeft.x = int (p1.x - p2.x/2);
bottomLeft.y = int (p1.y + p2.y/2);
bottomRight.x = int (p1.x + p2.x/2);
bottomRight.y = int (p1.y + p2.y/2);
rectangle(image, topLeft, bottomRight, cvScalar(204, 0, 204, 0),1,8,0);
line(image,topLeft, bottomRight, CV_RGB(204,0,204));
line(image,topRight, bottomLeft, CV_RGB(204,0,204));
}
//Draw LabB Location
circle(image, labB, 10, CV_RGB(255,0,0), 5, CV_AA, 0);
for(int y = 0; y < 6; y++)
{
for(int x = 0; x < 5; x++)
{
MapNode* mapNode = nodes[y][x];
cv::Point nodeLoc = mapNode->getPosition();
if(mapNode->getDown() != NULL)
line(image, mapNode->getPosition(), mapNode->getDown()->getPosition(), Scalar(0, 255, 0), 4, 6);
if(mapNode->getUp() != NULL)
line(image, mapNode->getPosition(), (mapNode->getUp())->getPosition(), Scalar(0, 255, 0), 4, 6);
if(mapNode->getLeft() != NULL)
line(image, mapNode->getPosition(), mapNode->getLeft()->getPosition(), Scalar(0, 255, 0), 4, 6);
if(mapNode->getRight() != NULL)
line(image, mapNode->getPosition(), mapNode->getRight()->getPosition(), Scalar(0, 255, 0), 4, 6);
}
}
cv :: Mat imageROI;
imageROI = image (cv :: Rect (pos.x, pos.y, logo.cols, logo.rows));
logo.copyTo (imageROI);
// for(int i = 0; i < p.size(); i++)
// {
// drawCross(p[i].getPosition(), Scalar(0, 255, 0), 5);
// }
cv :: imshow ("result", image);
}