Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add compass to simulator #201

Merged
merged 4 commits into from
Apr 20, 2016
Merged

Add compass to simulator #201

Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
fd3cb78
Add compass to simulator
cgmc Apr 16, 2016
d068533
Remove setting of uniqueID
cgmc Apr 16, 2016
91c8aa7
Adjust value of angleSlip
cgmc Apr 16, 2016
f7dbe3e
Account for implementation of modulo
cgmc Apr 19, 2016
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
42 changes: 30 additions & 12 deletions source/mega/simulator/simulator.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,14 +5,16 @@
#include <room.h>
#include "math.h"
#include <Shared_Structs.h>
#include <QueueList.h>

calc calculations;

typedef enum {
moveNum = 1,
stopNum = 2,
rotateNum = 3,
scanNum = 4
scanNum = 4,
compassNum = 5
} comNums;

const float SPEED = 1; //in mm per millisecond
Expand All @@ -23,7 +25,7 @@ Point MAP_BOUNDS[] = { {Point(0, 0)}, {Point(300, 0)}, {Point(300, 300)}, {Point
Room room = Room(4, MAP_BOUNDS, Point(STARTING_X, STARTING_Y), 1);

unsigned long startedMoving, moveTimer, scanTimer, rotateTimer, distTravelled;
int id, magnitude, movingAngle, laserAngle;
int magnitude, movingAngle, laserAngle, physicalAngle;
bool amScanning, amMoving, amRotating;

Point currentPosition, destination, terminus, nearestWall;
Expand All @@ -32,13 +34,14 @@ scanResponse scanResp;

float maxDegreeOfError = 100.0;
float degreeOfError = 0.0;
int angleSlip = 3;
int angleSlip = 20;

void setup() {
SPI_Wrapper::init();
SPI_Wrapper::registerMoveCommandHandler(&moveCommandHandler);
SPI_Wrapper::registerScanCommandHandler(&scanCommandHandler);
SPI_Wrapper::registerStopCommandHandler(&stopCommandHandler);
SPI_Wrapper::registerCompassCommandHandler(&compassCommandHandler);
Serial.begin(9600);
Serial.println("Starting...");
currentPosition.x = STARTING_X;
Expand Down Expand Up @@ -119,30 +122,41 @@ void scanCommandHandler(scanCommand scanCom) {
}
}

void compassCommandHandler(compassCommand compCom) {
if (com == NULL) {
com = new compassCommand(compCom);
}
}

void processCommand(command* com) {
if(com->commandNumber == moveNum){
if(com->commandNumber == moveNum) {
moveRobot((moveCommand*)com);
}
else if(com->commandNumber == stopNum){
else if(com->commandNumber == stopNum) {
amMoving = false;
unsigned long totalDistance = calculations.getDistBetweenTwoPoints(currentPosition, destination);
float distanceMoved = (((float)(millis() - startedMoving))/(float)(moveTimer - startedMoving))*(float)(totalDistance);

// Change currentPosition to represent the distance moved
currentPosition = calculations.makeLineFromPolar(movingAngle, (uint16_t)distanceMoved, currentPosition);
currentPosition = calculations.makeLineFromPolar(movingAngle, distanceMoved, currentPosition);

SPI_Wrapper::sendStopResponse(com->uniqueID, (uint16_t)distanceMoved, (uint16_t)movingAngle, true);
SPI_Wrapper::sendStopResponse(com->uniqueID, distanceMoved, movingAngle, true);
}
else if(com->commandNumber == rotateNum){
// rotate command to be implemented
}
else if(com->commandNumber == scanNum){
else if(com->commandNumber == scanNum) {
amScanning = true;
scanTimer = millis() + SCAN_RESPONSE_INTERVAL;
}
else if(com->commandNumber == compassNum) {
respond((compassCommand*)com);
delete com;
com = NULL;
}
}

void respond(moveCommand* com){
void respond(moveCommand* com) {
SPI_Wrapper::sendMoveResponse(com->uniqueID, distTravelled, movingAngle, true);
}

Expand All @@ -151,9 +165,14 @@ void respond(scanResponse scanResp) {
SPI_Wrapper::sendScanResponse(com->uniqueID, scanResp.angle, scanResp.magnitude, scanResp.last, true);
}

void respond(compassCommand* com) {
SPI_Wrapper::sendCompassResponse(com->uniqueID, ((physicalAngle + 270) % 360), true);
}

void moveRobot(moveCommand* com) {
destination = calculations.makeLineFromPolar((float((com->angle + 90) % 360) * PI) / 180 , com->magnitude, currentPosition);
movingAngle = float(com->angle) + (int)lround((angleSlip * (degreeOfError / maxDegreeOfError)));
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);
Line ray = Line(currentPosition, terminus);
terminus = calculations.getDestination(ray, room);
Expand All @@ -163,10 +182,9 @@ void moveRobot(moveCommand* com) {
moveTimer = millis() + calculations.getTravelTime(((com->magnitude) * 10), SPEED);
}

scanResponse scan(){
scanResponse scan() {
scanResponse scanResp;
scanResp.angle = laserAngle;
com->uniqueID = 4;
Line ray = Line(currentPosition, (calculations.makeLineFromPolar((((float)((laserAngle + 90) % 360) * PI) / 180), 4096.0, currentPosition)));
nearestWall = calculations.getDestination(ray, room);
scanResp.magnitude = (unsigned long)lround(calculations.getDistBetweenTwoPoints(ray.start, nearestWall));
Expand Down