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