/// <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;
}
/// <summary>
/// Handle periodic sensor readings from the pxbrick
/// </summary>
/// <param name="update"></param>
private IEnumerator<ITask> NotificationHandler(brick.LegoSensorUpdate update)
{
DriveState currentState = _state.Clone();
LegoResponseGetOutputState outputState = new LegoResponseGetOutputState(update.Body.CommandData);
if (outputState.Success && _state.RuntimeStatistics.LeftEncoderTimeStamp < outputState.TimeStamp)
{
LogVerbose(LogGroups.Console, outputState.ToString());
int leftReversePolaritySign = (currentState.LeftWheel.ReversePolarity) ? -1 : 1;
int rightReversePolaritySign = (currentState.LeftWheel.ReversePolarity) ? -1 : 1;
double direction, enctarget, encoder, remaining;
_state.TimeStamp = outputState.TimeStamp;
if (currentState.LeftWheel.MotorPort == outputState.MotorPort
&& currentState.RuntimeStatistics.LeftEncoderCurrent != (outputState.EncoderCount * leftReversePolaritySign))
{
if (currentState.LeftWheel.ReversePolarity)
{
outputState.EncoderCount *= -1;
outputState.BlockTachoCount *= -1;
outputState.ResettableCount *= -1;
}
currentState.RuntimeStatistics.LeftEncoderCurrent = outputState.EncoderCount;
_state.RuntimeStatistics.LeftEncoderCurrent = currentState.RuntimeStatistics.LeftEncoderCurrent;
_state.RuntimeStatistics.LeftEncoderTimeStamp = currentState.RuntimeStatistics.LeftEncoderTimeStamp = outputState.TimeStamp;
direction = Math.Sign(currentState.RuntimeStatistics.LeftPowerTarget);
enctarget = currentState.RuntimeStatistics.LeftEncoderTarget * direction;
encoder = outputState.EncoderCount * direction;
// negative remaining means we have passed the target.
remaining = enctarget - encoder;
if (enctarget != 0)
{
if (remaining <= 5 && _targetEncoderPending[LEFT])
{
_state.RuntimeStatistics.LeftPowerCurrent = 0.0;
_state.RuntimeStatistics.LeftPowerTarget = 0.0;
_targetEncoderReachedPort[LEFT].Post(true);
_targetEncoderPending[LEFT] = false;
if (_internalPendingDriveOperation == pxdrive.DriveRequestOperation.RotateDegrees)
{
_state.RuntimeStatistics.RightPowerCurrent = 0.0;
_state.RuntimeStatistics.RightPowerTarget = 0.0;
_targetEncoderReachedPort[RIGHT].Post(true);
_targetEncoderPending[RIGHT] = false;
}
}
}
}
if (currentState.RightWheel.MotorPort == outputState.MotorPort
&& currentState.RuntimeStatistics.RightEncoderCurrent != (outputState.EncoderCount * ((currentState.RightWheel.ReversePolarity) ? -1 : 1)))
{
if (currentState.RightWheel.ReversePolarity)
{
outputState.EncoderCount *= -1;
outputState.BlockTachoCount *= -1;
outputState.ResettableCount *= -1;
}
currentState.RuntimeStatistics.RightEncoderCurrent = outputState.EncoderCount;
_state.RuntimeStatistics.RightEncoderCurrent = currentState.RuntimeStatistics.RightEncoderCurrent;
_state.RuntimeStatistics.RightEncoderTimeStamp = currentState.RuntimeStatistics.RightEncoderTimeStamp = outputState.TimeStamp;
direction = Math.Sign(currentState.RuntimeStatistics.RightPowerTarget);
enctarget = currentState.RuntimeStatistics.RightEncoderTarget * direction;
encoder = outputState.EncoderCount * direction;
// negative remaining means we have passed the target.
remaining = enctarget - encoder;
if (enctarget != 0)
{
if (remaining <= 5 && _targetEncoderPending[RIGHT])
{
_state.RuntimeStatistics.RightPowerCurrent = 0.0;
_state.RuntimeStatistics.RightPowerTarget = 0.0;
_targetEncoderReachedPort[RIGHT].Post(true);
_targetEncoderPending[RIGHT] = false;
if (_internalPendingDriveOperation == pxdrive.DriveRequestOperation.RotateDegrees)
{
_state.RuntimeStatistics.LeftPowerCurrent = 0.0;
_state.RuntimeStatistics.LeftPowerTarget = 0.0;
_targetEncoderReachedPort[LEFT].Post(true);
_targetEncoderPending[LEFT] = false;
}
}
}
}
// Send encoder notifications.
SendNotification<DriveEncodersUpdate>(_subMgrPort, currentState.RuntimeStatistics);
}
yield break;
}
