bool dldCubesGame::playGameStep( Color mainColor )
{
initGame(mainColor);
if (waitForStepCompletion())
{
m_server.setLightColor( m_devices[0], mainColor.m_R, mainColor.m_G, mainColor.m_B );
m_server.setLightColor( m_devices[1], mainColor.m_R, mainColor.m_G, mainColor.m_B );
m_server.setLightColor( m_devices[2], mainColor.m_R, mainColor.m_G, mainColor.m_B );
}
else
{
m_server.setLightColor( m_devices[0], 0, 0, 0);
m_server.setLightColor( m_devices[1], 0, 0, 0);
m_server.setLightColor( m_devices[2], 0, 0, 0);
}
playSound();
waitForDistance();
}
void EmssController::move(int distance, int speed) {
if(mode == EmssController::EmergencyStop) return;
// Init
this->speed = speed;
angleToTurn = 0;
angleTurned = 0;
distanceToMove = distance;
distanceMoved = 0;
// Move, wait for distance, stop
mode = EmssController::Move;
waitForDistance();
mode = EmssController::Idle;
//TODO: why is this so inaccurate? What is the timing problem?
}
int moveToLocation(int trainNum, int source, int dest, track_node *track, int doReverse, int speed, int *velocity) {
struct Path path;
struct VelocityProfile profile;
BFS(source, dest, track, &path, doReverse);
if((path.node[path.numNodes-2])->reverse == path.node[path.numNodes-1]) {
path.numNodes--;
}
int periodic = Create(2, periodicTask);
initProfile(&profile, trainNum, speed, &path, source, periodic, velocity);
int curNode = 1;
profile.location = curNode-1;
int timeout = false;
int switchNode, switchDistance;
Putc2(1, (char)speed, (char)trainNum);
setAccelerating(&profile);
while(curNode < path.numNodes) {
if((path.node[curNode-1])->reverse == path.node[curNode]) {
waitForStop(&profile);
if(curNode != 1) {
profile.displayLocation = curNode;
profile.delta = -300000;
}
Putc2(1, (char)15, (char)trainNum);
Putc2(1, (char)speed, (char)trainNum);
setAccelerating(&profile);
}
profile.reverseNode = findNextReverseNode(&path, curNode);
int offset = 0;
while(curNode+offset < path.numNodes) {
switchNode = distanceBefore(&path, (profile.velocity)[speed]*50, curNode+offset, &switchDistance);
if((path.node[curNode+offset])->type == NODE_BRANCH && switchNode == curNode-1) {
if(switchDistance >= 0) {
waitForDistance(&profile, switchDistance);
}
if(adjDirection(path.node[curNode+offset], path.node[curNode+offset+1]) == DIR_STRAIGHT) {
setSwitchState((path.node[curNode+offset])->num, 'S');
}else{
setSwitchState((path.node[curNode+offset])->num, 'C');
}
}else if(switchNode > curNode-1) {
break;
}
offset++;
}
if(curNode == path.numNodes-1) break;
int distance = adjDistance(path.node[curNode-1], path.node[curNode]);
if(curNode == 1 && (path.node[0])->reverse == path.node[1]) {
}else if((path.node[curNode])->type == NODE_SENSOR) {
int reply;
int sensorNum = (path.node[curNode])->num;
int sensorTask = Create(2, sensorWaitTask);
Send(sensorTask, (char *)&sensorNum, sizeof(int), (char *)&reply, sizeof(int));
int src = waitForDistance(&profile, distance+150000);
if(src == periodic) {
printf("timeout at sensor %s\r", (path.node[curNode])->name);
timeout = true;
}else{
float err = profile.delta/1000 - distance/1000;
printf("distance error at node %s: %dmm\r", (path.node[curNode])->name, (int)err);
}
Destroy(sensorTask);
}else{
waitForDistance(&profile, distance);
}
setLocation(&profile, curNode);
curNode++;
if(timeout) {
profile.delta = 150000;
timeout = false;
}
}
int reply;
int sensorNum = (path.node[curNode])->num;
int sensorTask = Create(2, sensorWaitTask);
Send(sensorTask, (char *)&sensorNum, sizeof(int), (char *)&reply, sizeof(int));
int distance = adjDistance(path.node[curNode-1], path.node[curNode]);
int src = waitForDistanceOrStop(&profile, distance);
if(src == periodic) {
printf("timeout at sensor %s\r", (path.node[curNode])->name);
}else{
float err = profile.delta/1000 - distance/1000;
printf("distance error at node %s: %dmm\r", (path.node[curNode])->name, (int)err);
}
Destroy(sensorTask);
setLocation(&profile, path.numNodes-1);
waitForStop(&profile);
Destroy(periodic);
int curLocation = 0;
while(curLocation < TRACK_MAX && strcmp(track[curLocation].name,
(path.node[path.numNodes-1])->name) != 0) curLocation++;
return curLocation;
}
