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.");
}
}
}
}
