void Ship::correctSpeed() { float stopping_distance = sqrt(pow(getStoppingDistance(velocity.x, frame.getAccel()), 2.0f) + pow(getStoppingDistance(velocity.x, frame.getAccel()), 2.0f)); auto moving_toward = movingToward(target); if (stopping_distance < getDistanceBetween(getPosition(), target) - getGlobalBounds().width / 2.0f && moving_toward.first && moving_toward.second) { accelerate(); } else if (!moving_toward.first) { if (target.x > getPosition().x) { velocity.x += frame.getAccel() / 2.0f; } else { velocity.x -= frame.getAccel() / 2.0f; } } else if (!moving_toward.second) { if (target.y > getPosition().y) { velocity.y += frame.getAccel() / 2.0f; } else { velocity.y -= frame.getAccel() / 2.0f; } } else { decelerate(); } }
void driver() { Driver me; initDriver(&me, 1); unsigned int naggCount = 0; unsigned int updateStoppingDistanceCount = 0; for (;;) { int tid = -1; DriverMsg msg; msg.data2 = -1; msg.data3 = -1; msg.replyTid = -1; Receive(&tid, (char*)&msg, sizeof(DriverMsg)); if (tid != me.delayer && tid != me.stopDelayer) { Reply(tid, (char*)1, 0); } const int replyTid = msg.replyTid; switch (msg.type) { case SET_SPEED: { //TrainDebug(&me, "Set speed from msg"); trainSetSpeed(msg.data2, getStoppingTime(&me), (msg.data3 == DELAYER), &me); if (msg.data3 != DELAYER) { //TrainDebug(&me, "Replied to %d", replyTid); Reply(replyTid, (char*)1, 0); sendUiReport(&me); break; } else if (me.route.length != 0) { // Delayer came back. Reverse command completed me.stopCommited = 0; // We're moving again. // We've completed everything up to the reverse node. me.routeRemaining = me.stopNode+1; me.previousStopNode = me.routeRemaining; me.distanceFromLastSensorAtPreviousStopNode = me.distanceFromLastSensor; // Calculate the next stop node. updateStopNode(&me); me.nextSetSwitchNode = -1; updateSetSwitch(&me); // if the reverse is last node, nothing to do // if it isn't.. it should speed up again. } } case DELAYER: { //TrainDebug(&me, "delayer come back."); break; } case STOP_DELAYER: { // To prevent the first receive from this delayer if (me.lastSensorActualTime > 0 && me.speed == 0 && !me.isAding) { TrainDebug(&me, "releasing reserveration"); int reserveStatus = reserveMoreTrack(&me, 1, 0); if (reserveStatus == RESERVE_FAIL) { TrainDebug(&me, "WARNING: unable to reserve during init"); } } break; } case SENSOR_TRIGGER: { // only handle sensor reports in primary + secondary prediction if not position finding int sensorReportValid = 0; TrackLandmark conditionLandmark; int condition; int isSensorReserved = QueryIsSensorReserved(&me, msg.data2, msg.data3); if (me.positionFinding) { sensorReportValid = 1; me.lastSensorUnexpected = 1; //FinishPositionFinding(me.trainNum, me.trainController); } else if (isSensorReserved) { //TrainDebug(&me, "Predictions."); for (int i = 0; i < me.numPredictions; i ++) { TrackLandmark predictedSensor = me.predictions[i].sensor; //printLandmark(&me, &predictedSensor); if (predictedSensor.type == LANDMARK_SENSOR && predictedSensor.num1 == msg.data2 && predictedSensor.num2 == msg.data3) { sensorReportValid = 1; if (i != 0) { TrainDebug(&me, "Trigger Secondary"); // secondary prediction, need to do something about them conditionLandmark = me.predictions[i].conditionLandmark; condition = me.predictions[i].condition; me.lastSensorUnexpected = 1; if (conditionLandmark.type == LANDMARK_SWITCH) { TrackMsg setSwitch; setSwitch.type = UPDATE_SWITCH_STATE; TrainDebug(&me, "UPDATE SWITCH STATE"); setSwitch.landmark1 = conditionLandmark; setSwitch.data = condition; Send(me.trackManager, (char*)&setSwitch, sizeof(TrackMsg), (char *)1, 0); } // Stop and then try to reroute. reroute(&me); } else { me.lastSensorUnexpected = 0; } } } } if (sensorReportValid) { updateRoute(&me, msg.data2, msg.data3); me.lastSensorBox = msg.data2; // Box me.lastSensorVal = msg.data3; // Val me.lastSensorIsTerminal = 0; me.lastSensorActualTime = msg.timestamp; dynamicCalibration(&me); me.lastSensorPredictedTime = me.nextSensorPredictedTime; TrackNextSensorMsg trackMsg; QueryNextSensor(&me, &trackMsg); // Reserve the track above train and future (covers case of init) for (int i = 0; i < trackMsg.numPred; i++) { me.predictions[i] = trackMsg.predictions[i]; } me.numPredictions = trackMsg.numPred; int reserveStatus = reserveMoreTrack(&me, me.positionFinding, getStoppingDistance(&me)); if (reserveStatus == RESERVE_FAIL) { if (!me.positionFinding) { reroute(&me); } else { TrainDebug(&me, "WARNING: unable to reserve during init"); } } TrackSensorPrediction primaryPrediction = me.predictions[0]; me.calibrationStart = msg.timestamp; me.calibrationDistance = primaryPrediction.dist; int dPos = 50 * getVelocity(&me) / 100000.0; me.lastSensorDistanceError = -(int)me.distanceToNextSensor - dPos; me.distanceFromLastSensor = dPos; me.distanceToNextSensor = primaryPrediction.dist - dPos; me.lastPosUpdateTime = msg.timestamp; if (primaryPrediction.sensor.type != LANDMARK_SENSOR && primaryPrediction.sensor.type != LANDMARK_END) { TrainDebug(&me, "QUERY_NEXT_SENSOR_FROM_SENSOR ..bad"); } me.nextSensorIsTerminal = (primaryPrediction.sensor.type == LANDMARK_END); me.nextSensorBox = primaryPrediction.sensor.num1; me.nextSensorVal = primaryPrediction.sensor.num2; me.nextSensorPredictedTime = msg.timestamp + me.distanceToNextSensor*100000 / getVelocity(&me); updatePosition(&me, msg.timestamp); sendUiReport(&me); if (me.positionFinding) { trainSetSpeed(0, getStoppingTime(&me), 0, &me); // Found position, stop. me.positionFinding = 0; me.currentlyLost = 0; } } break; } case NAVIGATE_NAGGER: { updatePosition(&me, msg.timestamp); if (me.routeRemaining != -1) { if (!me.stopCommited) { if (shouldStopNow(&me)) { if (me.route.nodes[me.stopNode].num == REVERSE) { //TrainDebug(&me, "Navi reversing."); const int speed = -1; trainSetSpeed(speed, getStoppingTime(&me), 0, &me); } else { //TrainDebug(&me, "Navi Nagger stopping."); const int speed = 0; // Set speed zero. trainSetSpeed(speed, getStoppingTime(&me), 0, &me); me.route.length = 0; // Finished the route. me.testMode = 0; } me.stopCommited = 1; me.useLastSensorNow = 0; me.stopNow = 0; me.stopSensorHit = 0; } else { if ((++updateStoppingDistanceCount & 15) == 0) updateStopNode(&me); } } } if (me.nextSetSwitchNode != -1 && (++me.setSwitchNaggerCount & 3) == 0) { trySetSwitch_and_getNextSwitch(&me); } if (me.rerouteCountdown-- == 0) { if (me.testMode) { int reserveStatus = reserveMoreTrack(&me, 0, me.d[8][ACCELERATE][MAX_VAL]); // moving if (reserveStatus == RESERVE_FAIL) { reroute(&me); } else { me.nextSetSwitchNode = -1; updateSetSwitch(&me); trainSetSpeed(8, 0, 0, &me); } } else { // reroute if (me.route.length != 0) { setRoute(&me, &(me.routeMsg)); } } } if ((++naggCount & 15) == 0) sendUiReport(&me); break; } case SET_ROUTE: { Reply(replyTid, (char*)1, 0); me.routeMsg = msg; setRoute(&me, &msg); break; } case BROADCAST_UPDATE_PREDICTION: { updatePrediction(&me); int reserveStatus = reserveMoreTrack(&me, 0, getStoppingDistance(&me)); // moving if (reserveStatus == RESERVE_FAIL) { reroute(&me); } break; } case BROADCAST_TEST_MODE: { me.testMode = 1; setRoute(&me, &msg); break; } case FIND_POSITION: { me.positionFinding = 1; trainSetSpeed(5, 0, 0, &me); break; } default: { TrainDebug(&me, "Not suppported train message type."); } } } }