diff --git a/source/mega/simulator/libs/calc/calc.cpp b/source/mega/simulator/libs/calc/calc.cpp index ce75807..87f2cdb 100644 --- a/source/mega/simulator/libs/calc/calc.cpp +++ b/source/mega/simulator/libs/calc/calc.cpp @@ -15,23 +15,37 @@ float calc::getDistanceTravelled(float speed, unsigned long time) { return speed * time; } +bool calc::isWithinEpsilonOf(float value, float epsilon, float of) { + if(value >= (of - epsilon) && value <= (of + epsilon)) { + return true; + } + else { + return false; + } +} + +float calc::correctFloatErr(float value, float epsilon, float of) { + if(isWithinEpsilonOf(value, epsilon, of)) { + return of; + } + else { + return value; + } +} + Point calc::makeLineFromPolar(float angle, float distance, Point currentPosition) { Point temp; float xValueDelta = distance * cos(angle); - if(xValueDelta >= -0.012f && xValueDelta <= 0.012f) { - xValueDelta = 0.00; - } + xValueDelta = correctFloatErr(xValueDelta, 0.012f, 0.00f); temp.x = (currentPosition.x + xValueDelta); float yValueDelta = distance * sin(angle); - if(yValueDelta >= -0.012f && yValueDelta <= 0.012f) { - yValueDelta = 0.00; - } + yValueDelta = correctFloatErr(yValueDelta, 0.012f, 0.00f); temp.y = (currentPosition.y + yValueDelta); return temp; } bool calc::checkIfVertical(Point start, Point end) { - if((end.x - start.x) >= -0.01f && (end.x - start.x) <= 0.01f) { + if(isWithinEpsilonOf((end.x - start.x), 0.01f, 0.00f)) { return true; } else { @@ -49,7 +63,12 @@ float calc::getCOfLine(float slope, Point start) { Point calc::getDestination(Line robotLine, Room room) { //For each line, check for valid interception point, get interception point + //Serial.println("getDestination!"); Point nearestWall = robotLine.end; + /*Serial.print("Nearest wall: "); + Serial.print(nearestWall.x); + Serial.print(", "); + Serial.println(nearestWall.y);*/ Point validInterceptPoints[(room.numObjects) + 1][room.maxNumObjSides]; int indexVIP[(room.numObjects) + 1]; indexVIP[0] = 0; @@ -63,20 +82,16 @@ Point calc::getDestination(Line robotLine, Room room) { indexVIP[j] = 0; for(int k = 0; k < room.objects[j - 1].numSides; k++) { if(hasInterception(room.objects[j - 1].sides[k], robotLine)) { - validInterceptPoints[j][indexVIP[j]] = getInterceptPoint(robotLine, room.objects[j].sides[k]); + validInterceptPoints[j][indexVIP[j]] = getInterceptPoint(robotLine, room.objects[j - 1].sides[k]); indexVIP[j]++; } } } //Determine sign of each translation in x and y, relative to robot float diffInXValuesDest = robotLine.end.x - robotLine.start.x; - if(diffInXValuesDest >= -0.01f && diffInXValuesDest <= 0.01f) { - diffInXValuesDest = 0.00; - } + diffInXValuesDest = correctFloatErr(diffInXValuesDest, 0.01f, 0.00f); float diffInYValuesDest = robotLine.end.y - robotLine.start.y; - if(diffInYValuesDest >= -0.01f && diffInYValuesDest <= 0.01f) { - diffInYValuesDest = 0.00; - } + diffInYValuesDest = correctFloatErr(diffInYValuesDest, 0.01f, 0.00f); float diffInXValuesWall, diffInYValuesWall, distBetweenRobotAndDest, distBetweenRobotAndWall; //Get distance between robot and destination @@ -86,13 +101,9 @@ Point calc::getDestination(Line robotLine, Room room) { for(int j = 0; j < indexVIP[i]; j++) { //Determine sign of each translation for given interception point diffInXValuesWall = validInterceptPoints[i][j].x - robotLine.start.x; - if(diffInXValuesWall >= -0.01f && diffInXValuesWall <= 0.01f) { - diffInXValuesWall = 0.00; - } + diffInXValuesWall = correctFloatErr(diffInXValuesWall, 0.01f, 0.00f); diffInYValuesWall = validInterceptPoints[i][j].y - robotLine.start.y; - if(diffInYValuesWall >= -0.01f && diffInYValuesWall <= 0.01f) { - diffInYValuesWall = 0.00; - } + diffInYValuesWall = correctFloatErr(diffInYValuesWall, 0.01f, 0.00f); //If the signs match, we're facing the right direction 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) )) { @@ -106,61 +117,134 @@ Point calc::getDestination(Line robotLine, Room room) { } } } + /*Serial.print("Nearest wall: "); + Serial.print(nearestWall.x); + Serial.print(", "); + Serial.println(nearestWall.y);*/ return nearestWall; } bool calc::hasInterception(Line border, Line robotLine) { + /*Serial.println("hasInterception!"); + Serial.print("(("); + Serial.print(border.start.x); + Serial.print(", "); + Serial.print(border.start.y); + Serial.print("), ("); + Serial.print(border.end.x); + Serial.print(", "); + Serial.print(border.end.y); + Serial.println("))");*/ if((robotLine.isVertical && border.isVertical) || (robotLine.m == border.m && (!robotLine.isVertical || !border.isVertical))) { //Lines are parallel + //Serial.println("Lines are parallel!"); return false; } 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) - float intercept = ((robotLine.m * border.start.x) + ((-(robotLine.m * robotLine.start.x)) + robotLine.start.y)); + float xIntercept, yIntercept; if(robotLine.isVertical || border.isVertical) { if(robotLine.isVertical) { - return true; + /*Serial.println("Robot line is vertical!"); + Serial.print("Border start x: "); + Serial.println(border.start.x); + Serial.print("Robot x: "); + Serial.println(robotLine.start.x); + Serial.print("Border end x: "); + Serial.println(border.end.x);*/ + 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)) { + //Serial.println("In range!"); + return true; + } + else { + //Serial.println("Out of range!"); + return false; + } + //yIntercept = ((robotLine.m * border.start.x) + robotLine.c); } if(border.isVertical) { - if(intercept >= 0.0 && intercept <= 300.0) { + /*Serial.println("Wall is vertical!"); + Serial.print("Robot slope: "); + Serial.println(robotLine.m);*/ + if(border.start.x < robotLine.start.x) { + yIntercept = (robotLine.start.y + (-(robotLine.m) * abs(robotLine.start.x - border.start.x))); + } + else { + yIntercept = (robotLine.start.y + (robotLine.m * abs(robotLine.start.x - border.start.x))); + } + yIntercept = correctFloatErr(yIntercept, 0.01f, 0.00f); + /*Serial.print("Intercept with vertical line: "); + Serial.println(yIntercept);*/ + 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) ) ) ) { + //Serial.println("In range!"); return true; } else { + //Serial.println("Out of range!"); return false; } } } else { - return true; + /*Serial.println("Neither line is vertical!"); + Serial.print("Numerator: "); + Serial.println(border.c - robotLine.c); + Serial.print("Denominator: "); + Serial.println(robotLine.m - border.m);*/ + xIntercept = ((border.c - robotLine.c) / (robotLine.m - border.m)); + xIntercept = correctFloatErr(xIntercept, 0.01f, 0.00f); + /*Serial.print("X Intercept: "); + Serial.println(xIntercept);*/ + 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) ) ) ) { + //Serial.println("In range!"); + return true; + } + else { + //Serial.println("Out of range!"); + return false; + } } } } Point calc::getInterceptPoint(Line robotLine, Line other) { - Point intercept; - //Special case: one of the lines is vertical. - //In this event, the x-value of the intercept point will be constant for all values of y. - //Therefore, we can substitute that value into the equation of the non-vertical line and solve for y. - if(other.isVertical) { - intercept.x = other.start.x; - intercept.y = (robotLine.m * intercept.x) + robotLine.c; - } - else if(robotLine.isVertical) { - intercept.x = robotLine.start.x; - intercept.y = (other.m * intercept.x) + other.c; - } - 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; - float numerator = other.c - robotLine.c; - intercept.x = numerator / denominator; - //Substitute the x-value into either equation of the lines and solve for y - intercept.y = (robotLine.m * intercept.x) + robotLine.c; - if(intercept.y >= -0.01f && intercept.y <= 0.01f) { - intercept.y = 0.00; + //Serial.println("getInterceptPoint!"); + Point intercept; + /*Serial.print("(("); + Serial.print(other.start.x); + Serial.print(", "); + Serial.print(other.start.y); + Serial.print("), ("); + Serial.print(other.end.x); + Serial.print(", "); + Serial.print(other.end.y); + Serial.println("))");*/ + //Special case: one of the lines is vertical. + //In this event, the x-value of the intercept point will be constant for all values of y. + //Therefore, we can substitute that value into the equation of the non-vertical line and solve for y. + if(other.isVertical) { + intercept.x = other.start.x; + intercept.y = (robotLine.m * intercept.x) + robotLine.c; } - } - return intercept; + else if(robotLine.isVertical) { + intercept.x = robotLine.start.x; + intercept.y = (other.m * intercept.x) + other.c; + } + 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; + float numerator = other.c - robotLine.c; + intercept.x = numerator / denominator; + //Substitute the x-value into either equation of the lines and solve for y + intercept.y = (robotLine.m * intercept.x) + robotLine.c; + } + intercept.x = correctFloatErr(intercept.x, 0.01f, 0.00f); + intercept.y = correctFloatErr(intercept.y, 0.01f, 0.00f); + /*Serial.print("Intercept point: "); + Serial.print(intercept.x); + Serial.print(", "); + Serial.println(intercept.y);*/ + return intercept; } diff --git a/source/mega/simulator/libs/calc/calc.h b/source/mega/simulator/libs/calc/calc.h index f0bd9d1..8ba64a6 100644 --- a/source/mega/simulator/libs/calc/calc.h +++ b/source/mega/simulator/libs/calc/calc.h @@ -13,6 +13,8 @@ class calc { float getDistBetweenTwoPoints(Point p1, Point p2); float getTravelTime(unsigned long distance, float speed); float getDistanceTravelled(float speed, unsigned long time); + static bool isWithinEpsilonOf(float value, float epsilon, float of); + float correctFloatErr(float value, float epsilon, float of); Point makeLineFromPolar(float angle, float distance, Point currentPosition); static bool checkIfVertical(Point start, Point end); static float getSlopeOfLine(Point start, Point end); diff --git a/source/mega/simulator/libs/room/room.cpp b/source/mega/simulator/libs/room/room.cpp index 0252f79..a1226f5 100644 --- a/source/mega/simulator/libs/room/room.cpp +++ b/source/mega/simulator/libs/room/room.cpp @@ -1,5 +1,6 @@ #include "room.h" #include "calc.h"; +#include "Arduino.h"; Point::Point() { this->x = 0.0; diff --git a/source/mega/simulator/simulator.ino b/source/mega/simulator/simulator.ino index 6577393..25d6d6c 100644 --- a/source/mega/simulator/simulator.ino +++ b/source/mega/simulator/simulator.ino @@ -16,10 +16,10 @@ typedef enum { } comNums; const float SPEED = 1; //in mm per millisecond -const int STARTING_X = 30; //This and all distances measured in cm -const int STARTING_Y = 30; +const int STARTING_X = 70; //This and all distances measured in cm +const int STARTING_Y = 70; const unsigned long SCAN_RESPONSE_INTERVAL = 100; //Values as low as 80 worked in testing -Point MAP_BOUNDS[] = { {Point(0, 0)}, {Point(300, 0)}, {Point(300, 300)}, {Point(0, 300)} }; +Point MAP_BOUNDS[] = { {Point(0, 0)}, {Point(600, 0)}, {Point(600, 600)}, {Point(0, 600)} }; Room room = Room(4, MAP_BOUNDS, Point(STARTING_X, STARTING_Y), 1); unsigned long startedMoving, moveTimer, scanTimer, rotateTimer, distTravelled; @@ -46,7 +46,7 @@ void setup() { amScanning = false; amMoving = false; amRotating = false; - Point newObjHolder[] = { {Point(145, 145)}, {Point(155, 145)}, {Point(155, 155)}, {Point(145, 155)} }; + Point newObjHolder[] = { {Point(295, 295)}, {Point(305, 295)}, {Point(305, 305)}, {Point(295, 305)} }; Serial.print("Adding object 1: "); Serial.println(room.addObject(0, 4, newObjHolder)); scanTimer = millis(); @@ -66,11 +66,11 @@ void loop() { respond((moveCommand*)com); Serial.println("---------- Current Pos"); Serial.print(currentPosition.x); - Serial.println(", "); + Serial.print(", "); Serial.println(currentPosition.y); Serial.println("--------- Destination"); Serial.print(destination.x); - Serial.println(", "); + Serial.print(", "); Serial.println(destination.y); currentPosition = destination; amMoving = false; @@ -148,6 +148,11 @@ void respond(moveCommand* com){ void respond(scanResponse scanResp) { Serial.println("Sending Scan Response..."); + /*Serial.print("Angle: "); + Serial.print(scanResp.angle); + Serial.print("\t Distance: "); + Serial.print(scanResp.magnitude); + Serial.println();*/ SPI_Wrapper::sendScanResponse(com->uniqueID, scanResp.angle, scanResp.magnitude, scanResp.last, true); }