/// <summary>
/// Handle sensor update message from NXT
/// </summary>
public void SensorNotificationHandler(legoNXT.Configure notify)
{
if (notify == null)
{
throw new ArgumentNullException("notify");
}
//update time
_state.TimeStamp = DateTime.Now;
int newReading = notify.Body.MotorSensorPort[_state.HardwareIdentifier - 1];
//increment counter (and account for any missing ticks
if (Math.Abs(newReading - _state.CurrentReading) <= 3)
{
_state.TicksSinceReset += Math.Abs(newReading - _state.CurrentReading);
}
else
{
_state.TicksSinceReset++;
}
//Update angle
_state.CurrentReading = newReading;
//notify subscribers on any state change
encoder.UpdateTickCount updateHeader = new encoder.UpdateTickCount(new encoder.UpdateTickCountRequest(DateTime.Now, _state.TicksSinceReset));
SendNotification <encoder.UpdateTickCount>(_subMgrPort, updateHeader);
}
void StopMotorWithEncoderHandler(encoder.EncoderOperations encoderNotificationPort, string side, encoder.UpdateTickCount update, motor.MotorOperations motorPort)
{
//LogInfo("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: StopMotorWithEncoderHandler() " + side + " encoder at=" + update.Body.Count + " will stop wheel at=" + stopWheelAt);
int stopWheelAt;
bool ignore;
switch (side)
{
case "left":
stopWheelAt = stopLeftWheelAt;
ignore = !_leftEncoderTickEnabled;
break;
default:
case "right":
stopWheelAt = stopRightWheelAt;
ignore = !_rightEncoderTickEnabled;
break;
}
if (!ignore && update.Body.Count >= stopWheelAt)
{
switch (side)
{
case "left":
_leftEncoderTickEnabled = false;
break;
default:
case "right":
_rightEncoderTickEnabled = false;
break;
}
// whatever else got stuck there, we are not interested. Keep the port clear.
//Port<encoder.UpdateTickCount> port = (Port<encoder.UpdateTickCount>)encoderNotificationPort[typeof(encoder.UpdateTickCount)];
//port.Clear();
motor.SetMotorPower stop = new motor.SetMotorPower(new motor.SetMotorPowerRequest()
{
TargetPower = 0
});
motorPort.Post(stop);
Arbiter.Choice(stop.ResponsePort,
delegate(DefaultUpdateResponseType resp) {
LogInfo("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: StopMotorWithEncoderHandler() " + side + " - motor stopped !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
},
delegate(Fault fault) { LogError(fault); }
);
LogInfo("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: StopMotorWithEncoderHandler() " + side + " - Sending internal DriveStage.Completed !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
completionPort.Post(drive.DriveStage.Completed);
}
}
/// <summary>
/// Handle periodic sensor readings from the pxbrick
/// </summary>
/// <param name="update"></param>
private IEnumerator<ITask> NotificationHandler(brick.LegoSensorUpdate update)
{
LegoResponseGetOutputState outputState = new LegoResponseGetOutputState(update.Body.CommandData);
if (outputState.Success && _state.CurrentEncoderTimeStamp < outputState.TimeStamp)
{
LogVerbose(LogGroups.Console, outputState.ToString());
int reversePolaritySign = (_state.ReversePolarity) ? -1 : 1;
_rpmCalcPort.Post(outputState);
LogVerbose(LogGroups.Console, string.Format("{0} Encoder: RPM {1} AvgPoll {2}",
outputState.TimeStamp.ToString("HH:mm:ss.fffffff"),
_state.CurrentMotorRpm,
_state.AvgEncoderPollingRateMs,
outputState.RegulationMode
));
long newEncoderDegrees = outputState.EncoderCount * reversePolaritySign;
long newResettableEncoderDegrees = outputState.ResettableCount * reversePolaritySign;
bool encodersChanged = (_state.ResetableEncoderDegrees != newResettableEncoderDegrees);
// Update the current encoder readings as soon as possible.
_state.CurrentPower = ((double)(outputState.PowerSetPoint * reversePolaritySign)) / 100.0;
_state.CurrentEncoderTimeStamp = outputState.TimeStamp;
_state.CurrentEncoderDegrees = newEncoderDegrees;
_state.ResetableEncoderDegrees = newResettableEncoderDegrees;
if (encodersChanged)
{
// Prepare the encoder notification
pxencoder.UpdateTickCount tickNotification = new pxencoder.UpdateTickCount(
new pxencoder.UpdateTickCountRequest(outputState.TimeStamp, (int)newResettableEncoderDegrees));
SendNotification<pxencoder.UpdateTickCount>(_subMgrPort, tickNotification);
}
// get a snapshot of the current state.
MotorState currentState = _state.Clone();
#region Simple PD Control
if (currentState.TargetEncoderDegrees != 0)
{
double direction = Math.Sign(currentState.TargetPower);
double pwrtarget = currentState.TargetPower * direction;
double enctarget = currentState.TargetEncoderDegrees * direction;
double encoder = outputState.EncoderCount * direction;
double rpm = (double)currentState.CurrentMotorRpm * direction;
double msUntilNext = currentState.AvgEncoderPollingRateMs;
// negative remaining means we have passed the target.
double remaining = enctarget - encoder;
int goodEnough = (msUntilNext < 100) ? 5 : (msUntilNext < 300) ? 20 : 45;
bool targetReached = (remaining <= goodEnough);
LogVerbose(LogGroups.Console, string.Format("UpdateState: TargetPower {5} CurrentEncoder {0} {1} Target {2} done? {3} RPM {4} AvgMs {6}",
currentState.CurrentEncoderDegrees,
currentState.CurrentEncoderTimeStamp,
currentState.TargetEncoderDegrees,
targetReached,
currentState.CurrentMotorRpm,
currentState.TargetPower,
currentState.AvgEncoderPollingRateMs));
// If a prior encoder target was active, signal that it is cancelled.
if (_targetEncoderPending && targetReached)
{
lock (_targetEncoderReachedPort)
{
if (_targetEncoderPending)
{
LogVerbose(LogGroups.Console, "Signal target is complete!");
_targetEncoderReachedPort.Post(true);
_targetEncoderPending = false;
}
}
}
// How soon do we want to start slowing down?
// This is a factor of how fast and for how long the motor has been building up inertia.
double forwardLooking = (pwrtarget < 0.5) ? ((pwrtarget * 150.0) + 20) : ((pwrtarget * 200.0) + 20.0);
double degreesAtNextNotification = forwardLooking;
if (rpm != 0.0 && msUntilNext != 0.0)
// The distance in degrees we will travel at this rpm by the next encoder reading.
degreesAtNextNotification = (rpm * 360.0) * msUntilNext / 60000.0;
// We are not up-to-speed, wait a little longer before stopping
if (rpm < pwrtarget)
{
if (rpm > 0)
forwardLooking *= 0.7;
if (rpm < 0)
LogVerbose(LogGroups.Console, string.Format("Traveling in the wrong direction: Pwr {0} Enc {1} Tgt {2} RPM {3} Direction {4} FwdLook: {5} Remain: {6}",
pwrtarget, encoder, enctarget, rpm, direction, forwardLooking, remaining));
else
LogVerbose(LogGroups.Console, string.Format("Not up to speed: Pwr {0} Enc {1} Tgt {2} RPM {3} Direction {4} FwdLook: {5} Remain: {6}",
pwrtarget, encoder, enctarget, rpm, direction, forwardLooking, remaining));
}
else
{
LogVerbose(LogGroups.Console, string.Format("Targeting: Pwr {0} Enc {1} Tgt {2} RPM {3} Direction {4} FwdLook: {5} Remain: {6}",
pwrtarget, encoder, enctarget, rpm, direction, forwardLooking, remaining));
}
if (remaining < Math.Max(degreesAtNextNotification, forwardLooking)
|| remaining < forwardLooking
|| rpm <= 0)
{
LegoOutputMode outputMode = LegoOutputMode.MotorOn | LegoOutputMode.Regulated;
LegoRegulationMode regulationMode = LegoRegulationMode.Individual;
RunState powerAdjustment = RunState.Constant;
long tachoLimit = outputState.EncoderLimit;
int powerSetPoint = outputState.PowerSetPoint * (int)direction;
if (Math.Abs(remaining) < 5 && rpm == 0)
{
// If we are completely stopped,
// set the brake/coast and disengage active monitoring
// in the notification handler
_state.TargetEncoderDegrees = 0;
powerSetPoint = 0;
if (currentState.TargetStopState == MotorStopState.Brake)
{
outputMode |= LegoOutputMode.Brake;
}
else
{
regulationMode = LegoRegulationMode.Idle;
powerAdjustment = RunState.Idle;
}
}
else if ( remaining >= 0 &&
( (remaining < 100 && powerSetPoint > 0 && rpm > 30)
|| (remaining < goodEnough && rpm < 10)))
{
powerSetPoint = 0;
if (remaining < (pwrtarget / 2.0))
outputMode |= LegoOutputMode.Brake;
}
else if (forwardLooking < degreesAtNextNotification
&& (rpm >= pwrtarget) /* and up-to-speed */)
{
// we need to stop before the next encoder reading
powerSetPoint = 0;
outputMode |= LegoOutputMode.Brake;
// The ms until we reach the point where forwardlooking rotations are remaining.
double msUntilWeShouldStop = (degreesAtNextNotification - forwardLooking) / ((rpm * 360.0) / 60000.0);
yield return Timeout(msUntilWeShouldStop);
}
else if (remaining < 0)
{
// Overshot, reverse direction
powerSetPoint = CalculateMaxMotorPower((long)-remaining, -currentState.TargetPower, rpm, degreesAtNextNotification) / 2;
}
else
{
powerSetPoint = CalculateMaxMotorPower((long)remaining, currentState.TargetPower, rpm, degreesAtNextNotification);
}
if (degreesAtNextNotification < remaining)
powerAdjustment = RunState.RampDown;
if ((powerSetPoint != outputState.PowerSetPoint)
|| (outputState.Mode != outputMode)
|| (rpm < pwrtarget))
{
LogVerbose(LogGroups.Console, string.Format("Target: {0} Encoder: {1} New Power: {2}",
currentState.TargetEncoderDegrees,
newEncoderDegrees,
powerSetPoint));
#region Send an interim LEGO Motor command
LegoSetOutputState setOutputState = new LegoSetOutputState(currentState.MotorPort,
powerSetPoint,
outputMode,
regulationMode,
0, /* turn ratio */
powerAdjustment,
tachoLimit);
setOutputState.RequireResponse = (rpm == 0.0);
LogVerbose(LogGroups.Console, setOutputState.ToString());
yield return Arbiter.Choice(_legoBrickPort.SendNxtCommand(setOutputState),
delegate(LegoResponse ok)
{
if (ok.Success)
LogVerbose(LogGroups.Console, "Moto Power was set");
else
LogError(LogGroups.Console, "Moto Power Failed: " + ok.ErrorCode.ToString());
},
delegate(Fault fail)
{
LogError(LogGroups.Console, "Moto Power Failed: " + fail.ToString());
});
#endregion
}
}
else
{
LogVerbose(LogGroups.Console, string.Format("Remaining: {0} Current Power: {1} Forward Looking: {2}",
remaining,
currentState.TargetPower,
forwardLooking));
}
}
#endregion
}
yield break;
}