diff --git a/source/arduino/libraries/Simulator/calc/calc.cpp b/source/arduino/libraries/Simulator/calc/calc.cpp index 5206254..e16ff32 100644 --- a/source/arduino/libraries/Simulator/calc/calc.cpp +++ b/source/arduino/libraries/Simulator/calc/calc.cpp @@ -18,8 +18,7 @@ float calc::getDistanceTravelled(float speed, unsigned long time) { bool calc::isWithinEpsilonOf(float value, float epsilon, float of) { if(value >= (of - epsilon) && value <= (of + epsilon)) { return true; - } - else { + } else { return false; } } @@ -27,8 +26,7 @@ bool calc::isWithinEpsilonOf(float value, float epsilon, float of) { float calc::correctFloatErr(float value, float epsilon, float of) { if(isWithinEpsilonOf(value, epsilon, of)) { return of; - } - else { + } else { return value; } } @@ -47,8 +45,7 @@ Point calc::makeLineFromPolar(float angle, float distance, Point currentPosition bool calc::checkIfVertical(Point start, Point end) { if(isWithinEpsilonOf((end.x - start.x), 0.01f, 0.00f)) { return true; - } - else { + } else { return false; } } @@ -61,7 +58,7 @@ float calc::getCOfLine(float slope, Point start) { return(-(slope * start.x) + start.y); } -Point calc::getDestination(Line robotLine, Room room) { +Point calc::getDestination(Line robotLine, Room room, bool state) { //For each line, check for valid interception point, get interception point Point nearestWall = robotLine.end; Point validInterceptPoints[(room.numObjects) + 1][room.maxNumObjSides]; @@ -89,27 +86,11 @@ Point calc::getDestination(Line robotLine, Room room) { diffInYValuesDest = correctFloatErr(diffInYValuesDest, 0.01f, 0.00f); float diffInXValuesWall, diffInYValuesWall, distBetweenRobotAndDest, distBetweenRobotAndWall; - Point stepBack; //Get distance between robot and destination distBetweenRobotAndDest = getDistBetweenTwoPoints(robotLine.start, nearestWall); for(int i = 0; i < ((room.numObjects) + 1); i++) { for(int j = 0; j < indexVIP[i]; j++) { - //Check if near a wall or object, and trying to go through it - if(validInterceptPoints[i][j].x == robotLine.start.x && validInterceptPoints[i][j].y == robotLine.start.y) { - stepBack = Point(((2 * (robotLine.start.x - robotLine.end.x)) + robotLine.end.x), ((2 * (robotLine.start.y - robotLine.end.y)) + robotLine.end.y)); //P = d(B - A) + A where: - //A is the starting point (x0, y0) of the line segment, - //B is the end point (x1, y1), - //d is the distance from starting point A to the desired collinear point, - //and P is the desired collinear point - //Check signs of translations. If they match, we're trying to go through - diffInXValuesWall = validInterceptPoints[i][j].x - stepBack.x; - diffInYValuesWall = validInterceptPoints[i][j].y - stepBack.y; - if(( (diffInXValuesDest > 0.0 && diffInXValuesWall > 0.0) || (diffInXValuesDest == 0.0 && diffInXValuesWall == 0.0) || (diffInXValuesDest < 0.0 && diffInXValuesWall < 0.0) ) - &&( (diffInYValuesDest > 0.0 && diffInYValuesWall > 0.0) || (diffInYValuesDest == 0.0 && diffInYValuesWall == 0.0) || (diffInYValuesDest < 0.0 && diffInYValuesWall < 0.0) )) { - return robotLine.start; - } - } //Determine sign of each translation for given interception point diffInXValuesWall = validInterceptPoints[i][j].x - robotLine.start.x; diffInYValuesWall = validInterceptPoints[i][j].y - robotLine.start.y; @@ -128,14 +109,31 @@ Point calc::getDestination(Line robotLine, Room room) { } } } - return nearestWall; + if(state){ + Point stepBack; + for(int i = 0; i < ((room.numObjects) + 1); i++) { + for(int j = 0; j < indexVIP[i]; j++) { + //Check if near a wall or object, and trying to go through it + if(validInterceptPoints[i][j].x == nearestWall.x && validInterceptPoints[i][j].y == nearestWall.y) { + if(robotLine.isVertical){ + stepBack = Point(nearestWall.x, (nearestWall.y - 1.00f)); + } else{ + stepBack = makeLineFromPolar(atan(robotLine.m), (getDistBetweenTwoPoints(robotLine.start, validInterceptPoints[i][j]) - 1.00f), robotLine.start); + } + return stepBack; + } + } + } + return nearestWall; + } else{ + return nearestWall; + } } bool calc::hasInterception(Line border, Line robotLine) { if((robotLine.isVertical && border.isVertical) || (robotLine.m == border.m && (!robotLine.isVertical || !border.isVertical))) { //Lines are parallel return false; - } - else { + } else { //Line is in point-slope form (y = line.m(x - line.x) + line.y //Convert to slope-intercept form: y = (line.m * x) + (line.m * line.x) + line.y //y = line.m * x + ((line.m * line.x) + line.y) @@ -144,34 +142,29 @@ bool calc::hasInterception(Line border, Line robotLine) { if(robotLine.isVertical) { if((border.start.x <= robotLine.start.x && robotLine.start.x <= border.end.x) || (border.start.x >= robotLine.start.x && robotLine.start.x >= border.end.x)) { return true; - } - else { + } else { return false; } } if(border.isVertical) { if(border.start.x < robotLine.start.x) { yIntercept = (robotLine.start.y + (-(robotLine.m) * abs(robotLine.start.x - border.start.x))); - } - else { + } else { yIntercept = (robotLine.start.y + (robotLine.m * abs(robotLine.start.x - border.start.x))); } yIntercept = correctFloatErr(yIntercept, 0.01f, 0.00f); if( ( ( border.start.y < yIntercept || isWithinEpsilonOf(yIntercept, 0.01f, border.start.y) ) && ( yIntercept < border.end.y || isWithinEpsilonOf(yIntercept, 0.01f, border.end.y) ) ) || ( ( border.start.y > yIntercept || isWithinEpsilonOf(yIntercept, 0.01f, border.start.y) ) && ( yIntercept > border.end.y || isWithinEpsilonOf(yIntercept, 0.01f, border.end.y) ) ) ) { return true; - } - else { + } else { return false; } } - } - else { + } else { xIntercept = ((border.c - robotLine.c) / (robotLine.m - border.m)); xIntercept = correctFloatErr(xIntercept, 0.01f, 0.00f); if( ( ( border.start.x < xIntercept || isWithinEpsilonOf(xIntercept, 0.01f, border.start.x) ) && ( xIntercept < border.end.x || isWithinEpsilonOf(xIntercept, 0.01f, border.end.x) ) ) || ( ( border.start.x > xIntercept || isWithinEpsilonOf(xIntercept, 0.01f, border.start.x) ) && ( xIntercept > border.end.x || isWithinEpsilonOf(xIntercept, 0.01f, border.end.x) ) ) ) { return true; - } - else { + } else { return false; } } @@ -186,12 +179,10 @@ Point calc::getInterceptPoint(Line robotLine, Line other) { if(other.isVertical) { intercept.x = other.start.x; intercept.y = (robotLine.m * intercept.x) + robotLine.c; - } - else if(robotLine.isVertical) { + } else if(robotLine.isVertical) { intercept.x = robotLine.start.x; intercept.y = (other.m * intercept.x) + other.c; - } - else { + } else { //We set the two equations equal to each other and manipulate the expression to solve for x. // m1x + c1 = m2x + c2 -> m1x - m2x = c2 - c1 -> x = (c2 - c1) / (m1 - m2) float denominator = robotLine.m - other.m; diff --git a/source/arduino/libraries/Simulator/calc/calc.h b/source/arduino/libraries/Simulator/calc/calc.h index 8ba64a6..013a8be 100644 --- a/source/arduino/libraries/Simulator/calc/calc.h +++ b/source/arduino/libraries/Simulator/calc/calc.h @@ -19,7 +19,7 @@ class calc { static bool checkIfVertical(Point start, Point end); static float getSlopeOfLine(Point start, Point end); static float getCOfLine(float slope, Point start); - Point getDestination(Line robotLine, Room room); + Point getDestination(Line robotLine, Room room, bool state); //state is either moving (1) or scanning (0); bool hasInterception(Line border, Line robotLine); Point getInterceptPoint(Line robotLine, Line other); }; diff --git a/source/arduino/libraries/Simulator/room/room.cpp b/source/arduino/libraries/Simulator/room/room.cpp index 2966f06..56e89e5 100644 --- a/source/arduino/libraries/Simulator/room/room.cpp +++ b/source/arduino/libraries/Simulator/room/room.cpp @@ -7,9 +7,13 @@ Point::Point() { this->y = 0.0; } +Point::Point(float x, float y) { + this->x = x; + this->y = y; +} + Point::Point(int x, int y) { - this->x = (float)x; - this->y = (float)y; + Point((float)x, (float)y); } Line::Line() { @@ -49,6 +53,7 @@ Room::Room() { this->numObjects = 0; this->objects = new Object[this->numObjects]; this->maxNumObjSides = 4; + index = 0; } Room::Room(int numWalls, Point walls[], Point robotCoOrds, int numObjects) { @@ -57,15 +62,16 @@ Room::Room(int numWalls, Point walls[], Point robotCoOrds, int numObjects) { this->numObjects = numObjects; this->objects = new Object[numObjects]; this->maxNumObjSides = numWalls; + index = 0; } -bool Room::addObject(int index, int numSides, Point sides[]) { +bool Room::addObject(int numSides, Point sides[]) { if(index >= this->numObjects) { return false; - } - else { + } else { this->objects[index] = Object(numSides, sides); this->maxNumObjSides = max(maxNumObjSides, numSides); + index++; return true; } } diff --git a/source/arduino/libraries/Simulator/room/room.h b/source/arduino/libraries/Simulator/room/room.h index 7a6816e..3784312 100644 --- a/source/arduino/libraries/Simulator/room/room.h +++ b/source/arduino/libraries/Simulator/room/room.h @@ -6,6 +6,7 @@ class Point { public: Point(); + Point(float x, float y); Point(int x, int y); float x, y; }; @@ -28,6 +29,7 @@ class Object { }; class Room { + int index; public: Room(); Room(int numWalls, Point walls[], Point robotCoOrds, int numObjects); @@ -35,7 +37,7 @@ class Room { Object walls; int numObjects, maxNumObjSides; Object* objects; - bool addObject(int index, int numSides, Point sides[]); + bool addObject(int numSides, Point sides[]); }; #endif diff --git a/source/arduino/libraries/Simulator/room/roomTestCases.cpp b/source/arduino/libraries/Simulator/room/roomTestCases.cpp index 22e6a40..f5068f6 100644 --- a/source/arduino/libraries/Simulator/room/roomTestCases.cpp +++ b/source/arduino/libraries/Simulator/room/roomTestCases.cpp @@ -9,7 +9,7 @@ Room roomTestCases::pickRoom(int testCaseNum) { delete MAP_BOUNDS; MAP_BOUNDS = 0; newObjHolder = new Point[4]{Point(260, 260), Point(340, 260), Point(340, 340), Point(260, 340)}; - room.addObject(0, 4, newObjHolder); + room.addObject(4, newObjHolder); delete newObjHolder; newObjHolder = 0; return room; @@ -21,10 +21,10 @@ Room roomTestCases::pickRoom(int testCaseNum) { delete MAP_BOUNDS; MAP_BOUNDS = 0; newObjHolder = new Point[4]{Point(215, 260), Point(295, 260), Point(295, 340), Point(215, 340)}; - room.addObject(0, 4, newObjHolder); + room.addObject(4, newObjHolder); delete newObjHolder; newObjHolder = new Point[4]{Point(305, 260), Point(385, 260), Point(385, 340), Point(305, 340)}; - room.addObject(1, 4, newObjHolder); + room.addObject(4, newObjHolder); delete newObjHolder; newObjHolder = 0; return room; diff --git a/source/mega/simulator/simulator.ino b/source/mega/simulator/simulator.ino index 2a67d2b..821e97e 100644 --- a/source/mega/simulator/simulator.ino +++ b/source/mega/simulator/simulator.ino @@ -25,9 +25,9 @@ Room room; unsigned long startedMoving, moveTimer, scanTimer, rotateTimer, distTravelled; int magnitude, movingAngle, laserAngle, physicalAngle; -bool amScanning, amMoving, amRotating; +bool amScanning, amMoving, amRotating, bump; -Point destination, terminus, nearestWall; +Point terminus, endpoint, nearestWall; command* com; scanResponse scanResp; @@ -57,8 +57,7 @@ void loop() { if (com != NULL) { if(!amMoving && !amScanning) { processCommand(com); - } - else if (amMoving && com->commandNumber == stopNum) { + } else if (amMoving && com->commandNumber == stopNum) { processCommand(com); } } @@ -69,12 +68,12 @@ void loop() { Serial.print(", "); Serial.println(room.robot.y); Serial.println("--------- Destination"); - Serial.print(destination.x); + Serial.print(terminus.x); Serial.print(", "); - Serial.println(destination.y); + Serial.println(terminus.y); delay(50);*/ respond((moveCommand*)com); - room.robot = terminus; + room.robot = endpoint; amMoving = false; delete com; com = NULL; @@ -136,27 +135,23 @@ void compassCommandHandler(compassCommand compCom) { void processCommand(command* com) { if(com->commandNumber == moveNum) { moveRobot((moveCommand*)com); - } - else if(com->commandNumber == stopNum) { + } else if(com->commandNumber == stopNum) { amMoving = false; - unsigned long totalDistance = calculations.getDistBetweenTwoPoints(room.robot, destination); - float distanceMoved = (((float)(millis() - startedMoving))/(float)(moveTimer - startedMoving))*(float)(totalDistance); + unsigned long totalDistance = calculations.getDistBetweenTwoPoints(room.robot, terminus); + distTravelled = (((millis() - startedMoving))/(moveTimer - startedMoving))*(totalDistance); // Change robot's position in room to represent the distance moved - room.robot = calculations.makeLineFromPolar(movingAngle, distanceMoved, room.robot); + room.robot = calculations.makeLineFromPolar(movingAngle, distTravelled, room.robot); - SPI_Wrapper::sendStopResponse(com->uniqueID, distanceMoved, movingAngle, true); - } - else if(com->commandNumber == rotateNum) { + SPI_Wrapper::sendStopResponse(com->uniqueID, distTravelled, movingAngle, true); + } else if(com->commandNumber == rotateNum) { respond((rotateCommand*)com); delete com; com = NULL; - } - else if(com->commandNumber == scanNum) { + } else if(com->commandNumber == scanNum) { amScanning = true; scanTimer = millis() + SCAN_RESPONSE_INTERVAL; - } - else if(com->commandNumber == compassNum) { + } else if(com->commandNumber == compassNum) { respond((compassCommand*)com); delete com; com = NULL; @@ -164,7 +159,7 @@ void processCommand(command* com) { } void respond(moveCommand* com){ - SPI_Wrapper::sendMoveResponse(com->uniqueID, distTravelled, movingAngle, (distTravelled == com->magnitude)); + SPI_Wrapper::sendMoveResponse(com->uniqueID, distTravelled, movingAngle, bump); } void respond(rotateCommand* com) { @@ -187,14 +182,18 @@ void respond(compassCommand* com) { } void moveRobot(moveCommand* com) { + Point wallGuard, destination; + amMoving = true; destination = calculations.makeLineFromPolar(((float)(((90 - com->angle) + 360) % 360) * PI) / 180 , com->magnitude, room.robot); movingAngle = com->angle + (int)lround((angleSlip * (degreeOfError / maxDegreeOfError))); physicalAngle = physicalAngle + (int)lround((angleSlip * (degreeOfError / maxDegreeOfError))); terminus = calculations.makeLineFromPolar(((float)(((90 - movingAngle) + 360) % 360) * PI) / 180, com->magnitude, room.robot); Line ray = Line(room.robot, terminus); - terminus = calculations.getDestination(ray, room); + wallGuard = terminus; + terminus = calculations.getDestination(ray, room, amMoving); + bump = (wallGuard.x != terminus.x || wallGuard.y != terminus.y); distTravelled = (unsigned long)lround(calculations.getDistBetweenTwoPoints(ray.start, terminus) * (1.0 - (degreeOfError / maxDegreeOfError))); - amMoving = true; + endpoint = calculations.makeLineFromPolar(((float)(((90 - movingAngle) + 360) % 360) * PI) / 180, distTravelled, room.robot); startedMoving = millis(); moveTimer = millis() + calculations.getTravelTime(((com->magnitude) * 10), SPEED); } @@ -203,7 +202,7 @@ scanResponse scan() { scanResponse scanResp; scanResp.angle = laserAngle; Line ray = Line(room.robot, (calculations.makeLineFromPolar((((float)(((90 - laserAngle) + 360) % 360) * PI) / 180), 4096.0, room.robot))); - nearestWall = calculations.getDestination(ray, room); + nearestWall = calculations.getDestination(ray, room, amMoving); scanResp.magnitude = (unsigned long)lround(calculations.getDistBetweenTwoPoints(ray.start, nearestWall)); scanResp.last = (laserAngle == 360); return scanResp;