diff --git a/source/mega/simulator/simulator.ino b/source/mega/simulator/simulator.ino index 64fbaa9..706fe12 100644 --- a/source/mega/simulator/simulator.ino +++ b/source/mega/simulator/simulator.ino @@ -173,7 +173,7 @@ void moveRobot(moveCommand* com) { destination = calculations.makeLineFromPolar((float((com->angle + 90) % 360) * PI) / 180 , com->magnitude, currentPosition); movingAngle = com->angle + (int)lround((angleSlip * (degreeOfError / maxDegreeOfError))); physicalAngle = physicalAngle + (int)lround((angleSlip * (degreeOfError / maxDegreeOfError))); - terminus = calculations.makeLineFromPolar((float((movingAngle + 90) % 360) * PI) / 180, com->magnitude, currentPosition); + terminus = calculations.makeLineFromPolar(((float)(((90 - movingAngle) + 360) % 360) * PI) / 180, com->magnitude, currentPosition); Line ray = Line(currentPosition, terminus); terminus = calculations.getDestination(ray, room); distTravelled = (unsigned long)lround(calculations.getDistBetweenTwoPoints(ray.start, terminus) * (1.0 - (degreeOfError / maxDegreeOfError))); @@ -185,7 +185,7 @@ void moveRobot(moveCommand* com) { scanResponse scan() { scanResponse scanResp; scanResp.angle = laserAngle; - Line ray = Line(currentPosition, (calculations.makeLineFromPolar((((float)((laserAngle + 90) % 360) * PI) / 180), 4096.0, currentPosition))); + Line ray = Line(currentPosition, (calculations.makeLineFromPolar((((float)(((90 - laserAngle) + 360) % 360) * PI) / 180), 4096.0, currentPosition))); nearestWall = calculations.getDestination(ray, room); scanResp.magnitude = (unsigned long)lround(calculations.getDistBetweenTwoPoints(ray.start, nearestWall)); scanResp.last = (laserAngle == 360);