Prevent robot moving into wall

Instead of allowing the robot to move into a wall or side of object, and
waiting until a subsequent move to see if that move is going through the
wall or side of object, the code now checks after the destination has
been determined, to see if that destination lies on a wall or side of
object, and if so, adjusts the destination to a point at a shorter
distance.
Because scanning relies on the destination being a point on a wall or
side of object, a bool was added to the getDestination function to
differentiate between the two states.
A new Point constructor was created that takes two floats directly, and
the existing constructor was re-written to call this new constructor.

If there was any error in the simulator, the move response would always
return a status of false, which was undesired behaviour. The code has
been adjusted so that it only returns false if the robot would have hit
a wall or side of object.

The addObject function was altered so that index could be a private
variable, since there would never be a situation where an object would
be replaced mid-runtime - a different room would be created instead.

The stop response code was corrected to use the global distTravelled
variable instead of its own variable.

Finally, some preliminary edits in relation to indentation were made,
to comply with the repository style guide.

References #164.

source/arduino/libraries/Simulator/calc/calc.cpp

@@ -18,17 +18,15 @@ 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;
 	}
 }
 
 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;

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);
 };

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;
 	}
 }

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,14 +29,15 @@ class Object {
 };
 
 class Room {
+	int index;
 	public:
 		Room();
 		Room(int numWalls, Point walls[], Point robotCoOrds, int numObjects);
 		Point robot;
 		Object walls;
 		int numObjects, maxNumObjSides;
 		Object* objects;
-		bool addObject(int index, int numSides, Point sides[]);
+		bool addObject(int numSides, Point sides[]);
 };
 
 #endif

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;

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,35 +135,31 @@ 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;
   }
 }
 
 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;