Skip to content

Commit

Permalink
Fix multiple bugs, including object detection
Browse files Browse the repository at this point in the history 
Object detection remained broken because getDestination() was accessing
the j'th index in the object array, which was one ahead of where it
should have been accessing.

Rooms larger than 30 cm square could not be properly created and used
because hasInterception() was using a hard-coded boundary check which
assumed that the room was 30 cm square. The function has been
overhauled, and now uses the start and end points of the line in
question in its boundary checks. The room has also been adjusted to test
this - it is now 60 cm square, and the single object in the room has
been moved accordingly.

The room class does not automatically have access to a max() function -
it must be referenced from the Arduino header.

Finally, I noticed that there was some code duplication in regards to
float comparison, so I created two new functions to consolidate the
code.

References #162.
  • Loading branch information
@cgmc
cgmc committed Apr 16, 2016
1 parent 5f01959 commit 8a1af90
Show file tree
Hide file tree
Showing 4 changed files with 146 additions and 54 deletions.
180 changes: 132 additions & 48 deletions source/mega/simulator/libs/calc/calc.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand All @@ -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;
Expand All @@ -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
Expand All @@ -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) )) {
Expand All @@ -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;
}
2 changes: 2 additions & 0 deletions source/mega/simulator/libs/calc/calc.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
1 change: 1 addition & 0 deletions source/mega/simulator/libs/room/room.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
#include "room.h"
#include "calc.h";
#include "Arduino.h";

Point::Point() {
this->x = 0.0;
Expand Down
17 changes: 11 additions & 6 deletions source/mega/simulator/simulator.ino
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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();
Expand All @@ -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;
Expand Down Expand Up @@ -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);
}

Expand Down

0 comments on commit 8a1af90

Please sign in to comment.