/// <summary>
/// Internal Motor Rotation Handler
/// Exclusive handler which controls all motor commands
/// Responds to a flood of incoming motor commands by
/// only processing the newest command.
/// </summary>
/// <param name="setMotorPower"></param>
/// <returns></returns>
public virtual IEnumerator<ITask> InternalMotorRotationHandler(SetMotorRotation setMotorPower)
{
try
{
#region Check for a backlog of motor commands
// Take a snapshot of the number of pending commands at the time
// we entered this routine.
// This will prevent a livelock which can occur if we try to
// process the queue until it is empty, but the inbound queue
// is growing faster than we are pulling off the queue.
int pendingCommands = _internalMotorRotationPort.ItemCount;
// If newer commands have been issued,
// respond fault to the older command and
// move to the newer one.
SetMotorRotation newerRotationRequest;
Fault faultNewerCommand = Fault.FromException(new InvalidOperationException("A newer motor command was received before this command was processed."));
while (pendingCommands > 0)
{
if (_internalMotorRotationPort.Test(out newerRotationRequest))
{
setMotorPower.ResponsePort.Post(faultNewerCommand);
setMotorPower = newerRotationRequest;
}
pendingCommands--;
}
#endregion
// Get a snapshot of our state.
MotorState currentState = _state.Clone();
Fault faultResponse = null;
int motorPower;
long targetEncoderDegrees = 0;
long targetRotationDegrees;
CalculatePowerAndTargetDegrees(setMotorPower.Body, currentState.ReversePolarity,
out motorPower,
out targetRotationDegrees);
#region Cancel any prior encoder target
// If a prior encoder target was active,
// signal that it is cancelled by a new Motor command.
if (_targetEncoderPending)
{
lock (_targetEncoderReachedPort)
{
if (_targetEncoderPending)
{
LogVerbose(LogGroups.Console, string.Format("Cancel prior target! New TgtDegrees: {0}", targetRotationDegrees));
_targetEncoderReachedPort.Post(false);
_targetEncoderPending = false;
_state.TargetEncoderDegrees = 0;
_state.CurrentEncoderTimeStamp = DateTime.Now;
}
}
}
#endregion
_state.TargetPower = setMotorPower.Body.TargetPower;
if (_state.TargetPower == 0)
SendNotification<AllStop>(_subMgrPort, new AllStop(setMotorPower.Body.StopState));
// If rotating for a specific number of rotations,
// set up the extended transaction and get the most recent encoder reading.
if (targetRotationDegrees != 0)
{
#region If encoder reading is old, update the encoder now.
if (DateTime.Now.Subtract(currentState.CurrentEncoderTimeStamp).TotalMilliseconds > 45)
{
LegoGetOutputState getEncoder = new LegoGetOutputState(currentState.MotorPort);
getEncoder.TryCount = 2;
yield return Arbiter.Choice(_legoBrickPort.SendNxtCommand(getEncoder),
delegate(LegoResponse legoResponse)
{
LegoResponseGetOutputState encoderResponse = legoResponse as LegoResponseGetOutputState;
if (encoderResponse == null) encoderResponse = new LegoResponseGetOutputState(legoResponse.CommandData);
if (encoderResponse.Success)
{
int reversePolaritySign = (currentState.ReversePolarity) ? -1 : 1;
_state.CurrentPower = ((double)(encoderResponse.PowerSetPoint * reversePolaritySign)) / 100.0;
_state.CurrentEncoderTimeStamp = encoderResponse.TimeStamp;
_state.CurrentEncoderDegrees = encoderResponse.EncoderCount * reversePolaritySign;
_state.ResetableEncoderDegrees = encoderResponse.ResettableCount * reversePolaritySign;
currentState = _state.Clone();
}
else
{
LogError(LogGroups.Console, "Failed to set motor: " + legoResponse.ErrorCode.ToString());
faultResponse = Fault.FromException(new InvalidOperationException(legoResponse.ErrorCode.ToString()));
}
},
delegate(Fault fault)
{
LogError(LogGroups.Console, fault.ToString());
faultResponse = fault;
});
if (faultResponse != null)
{
setMotorPower.ResponsePort.Post(faultResponse);
yield break;
}
}
#endregion
// Calcuate a new target encoder which is based on the current encoder position.
targetEncoderDegrees = targetRotationDegrees + currentState.CurrentEncoderDegrees;
// A target of zero diables the PD control. Make sure our result isn't 0.
if (targetEncoderDegrees == 0) targetEncoderDegrees++;
LogVerbose(LogGroups.Console, string.Format("*** Target Degrees {0} Encoder {1}", targetRotationDegrees, targetEncoderDegrees));
_state.TargetEncoderDegrees = targetEncoderDegrees;
_state.TargetStopState = setMotorPower.Body.StopState;
lock (_targetEncoderReachedPort)
{
_targetEncoderPending = true;
}
}
else
{
_state.TargetEncoderDegrees = 0;
}
LogVerbose(LogGroups.Console, string.Format("{0} SetMotorPower: {1} Encoder: {2} AdjPwr: {3} TgtRot: {4} TgtEnc {5}",
DateTime.Now.ToString("HH:mm:ss.fffffff"),
motorPower,
currentState.CurrentEncoderDegrees,
motorPower,
targetRotationDegrees,
targetEncoderDegrees));
#region Set up the LEGO Motor Command
// If we aren't polling, let the motor regulate it's own distance.
int motorRotationLimit = 0;
if (targetRotationDegrees != 0 && (currentState.PollingFrequencyMs <= 0 || currentState.PollingFrequencyMs > 100))
motorRotationLimit = (int)targetRotationDegrees;
LegoRegulationMode regulationMode = LegoRegulationMode.Individual;
LegoOutputMode outputMode = LegoOutputMode.MotorOn | LegoOutputMode.Regulated;
RunState powerAdjustment = (setMotorPower.Body.RampUp && targetRotationDegrees != 0) ? RunState.RampUp : RunState.Constant;
// Are we stopping?
if (setMotorPower.Body.TargetPower == 0.0)
{
motorRotationLimit = 0;
switch (setMotorPower.Body.StopState)
{
case MotorStopState.Brake:
outputMode = LegoOutputMode.MotorOn | LegoOutputMode.Regulated | LegoOutputMode.Brake;
powerAdjustment = RunState.Constant;
break;
case MotorStopState.Coast:
powerAdjustment = RunState.Idle;
regulationMode = LegoRegulationMode.Idle;
break;
}
}
// Set up the LEGO motor command
LegoSetOutputState motorCmd = new LegoSetOutputState(
currentState.MotorPort,
motorPower,
outputMode,
regulationMode,
0, /* Turn ratio */
powerAdjustment,
motorRotationLimit);
#endregion
#region Send the LEGO Motor Command
bool abort = false;
LogVerbose(LogGroups.Console, motorCmd.ToString());
yield return Arbiter.Choice(_legoBrickPort.SendNxtCommand(motorCmd),
delegate(LegoResponse response)
{
if (!response.Success)
{
LogError(LogGroups.Console, "Invalid SetMotorOutput " + response.ErrorCode.ToString());
abort = true;
setMotorPower.ResponsePort.Post(
Fault.FromException(
new InvalidOperationException(response.ErrorCode.ToString())));
}
},
delegate(Fault fault)
{
abort = true;
LogError(LogGroups.Console, "Fault in SetMotorOutput " + fault.ToString());
setMotorPower.ResponsePort.Post(fault);
});
#endregion
if (abort)
yield break;
// Send a notification that the power has been set.
SendNotification<SetMotorRotation>(_subMgrPort, setMotorPower);
// Wait for the target encoder to either be cancelled or reach its target.
if (_targetEncoderPending)
{
LogVerbose(LogGroups.Console, "Waiting for motor to reach target...");
// Wait for the target encoder to either be cancelled or reached.
Activate(Arbiter.Receive<bool>(false, _targetEncoderReachedPort,
delegate(bool finished)
{
RespondToMotorPowerRequest(setMotorPower.ResponsePort, finished);
}));
}
else
{
RespondToMotorPowerRequest(setMotorPower.ResponsePort, true);
}
yield break;
}
finally
{
Activate(Arbiter.ReceiveWithIterator<SetMotorRotation>(false, _internalMotorRotationPort, InternalMotorRotationHandler));
}
}
/// <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>
/// Process the most recent Drive Power command
/// When complete, self activate for the next internal command
/// </summary>
/// <param name="driveDistance"></param>
/// <returns></returns>
public virtual IEnumerator<ITask> InternalDrivePowerHandler(DriveDistance driveDistance)
{
try
{
#region Check for a backlog of drive commands
// Take a snapshot of the number of pending commands at the time
// we entered this routine.
// This will prevent a livelock which can occur if we try to
// process the queue until it is empty, but the inbound queue
// is growing at the same rate as we are pulling off the queue.
int pendingCommands = _internalDrivePowerPort.ItemCount;
// If newer commands have been issued,
// respond success to the older command and
// move to the newer one.
DriveDistance newerUpdate;
while (pendingCommands > 0)
{
if (_internalDrivePowerPort.Test(out newerUpdate))
{
driveDistance = newerUpdate;
}
pendingCommands--;
}
#endregion
// Get a snapshot of our state.
DriveState currentState = _state.Clone();
#region Cancel any prior encoder target
// If a prior encoder target was active,
// signal that it is cancelled by a new Motor command.
if (_targetEncoderPending[LEFT] || _targetEncoderPending[RIGHT])
{
lock (_targetEncoderPending)
{
LogVerbose(LogGroups.Console, "Cancel prior target!");
if (_targetEncoderPending[LEFT])
{
_targetEncoderReachedPort[LEFT].Post(false);
_targetEncoderPending[LEFT] = false;
}
if (_targetEncoderPending[RIGHT])
{
_targetEncoderReachedPort[RIGHT].Post(false);
_targetEncoderPending[RIGHT] = false;
}
_state.RuntimeStatistics.LeftEncoderTarget = 0;
_state.RuntimeStatistics.RightEncoderTarget = 0;
}
}
#endregion
// Calculate the motor power and target degrees for both motors.
int leftMotorPower, rightMotorPower;
long leftTargetEncoderDegrees, rightTargetEncoderDegrees;
CalculatePowerAndTargetDegrees(driveDistance.Body, currentState,
out leftMotorPower,
out rightMotorPower,
out leftTargetEncoderDegrees,
out rightTargetEncoderDegrees);
_state.RuntimeStatistics.LeftPowerCurrent = ((double)leftMotorPower) / 100.0; ;
_state.RuntimeStatistics.LeftPowerTarget = driveDistance.Body.LeftPower;
_state.RuntimeStatistics.RightPowerCurrent = ((double)rightMotorPower) / 100.0;
_state.RuntimeStatistics.RightPowerTarget = driveDistance.Body.RightPower;
LegoOutputMode leftOutputMode = (leftMotorPower == 0 || leftTargetEncoderDegrees != 0) ? LegoOutputMode.PowerBrake : LegoOutputMode.PowerRegulated;
LegoOutputMode rightOutputMode = (rightMotorPower == 0 || rightTargetEncoderDegrees != 0) ? LegoOutputMode.PowerBrake : LegoOutputMode.PowerRegulated;
if (leftTargetEncoderDegrees != 0)
{
// Calcuate a new target encoder which is based on the current encoder position.
leftTargetEncoderDegrees += currentState.RuntimeStatistics.LeftEncoderCurrent;
// A target of zero diables the PD control. Make sure our result isn't 0.
if (leftTargetEncoderDegrees == 0) leftTargetEncoderDegrees++;
_state.RuntimeStatistics.TargetStopState = driveDistance.Body.StopState;
_targetEncoderPending[LEFT] = true;
}
_state.RuntimeStatistics.LeftEncoderTarget = leftTargetEncoderDegrees;
if (rightTargetEncoderDegrees != 0)
{
// Calcuate a new target encoder which is based on the current encoder position.
rightTargetEncoderDegrees += currentState.RuntimeStatistics.RightEncoderCurrent;
// A target of zero diables the PD control. Make sure our result isn't 0.
if (rightTargetEncoderDegrees == 0) rightTargetEncoderDegrees++;
_state.RuntimeStatistics.TargetStopState = driveDistance.Body.StopState;
_targetEncoderPending[RIGHT] = true;
}
_state.RuntimeStatistics.RightEncoderTarget = rightTargetEncoderDegrees;
_state.TimeStamp = DateTime.Now;
LegoRegulationMode syncMode = LegoRegulationMode.Individual;
// Send the left motor command to the brick
LegoSetOutputState motorCmd = new LegoSetOutputState(
currentState.LeftWheel.MotorPort,
leftMotorPower,
leftOutputMode,
syncMode,
0, // turnratio
RunState.Constant,
driveDistance.Body.LeftStopAtRotationDegrees);
motorCmd.RequireResponse = false;
LogVerbose(LogGroups.Console, motorCmd.ToString());
Fault fault = null;
PortSet<LegoResponse, Fault> leftResponse = _legoBrickPort.SendNxtCommand(motorCmd);
// Send the right motor comand to the brick
motorCmd.MotorPort = currentState.RightWheel.MotorPort;
motorCmd.PowerSetPoint = rightMotorPower;
motorCmd.Mode = rightOutputMode;
motorCmd.TurnRatio = 0;
motorCmd.EncoderLimit = driveDistance.Body.RightStopAtRotationDegrees;
LogVerbose(LogGroups.Console, motorCmd.ToString());
PortSet<LegoResponse, Fault> rightResponse = _legoBrickPort.SendNxtCommand(motorCmd);
yield return Arbiter.Choice(leftResponse,
delegate(LegoResponse response)
{
if (!response.Success)
fault = Fault.FromException(
new InvalidOperationException(response.ErrorCode.ToString()));
},
delegate(Fault f)
{
fault = f;
});
yield return Arbiter.Choice(rightResponse,
delegate(LegoResponse response)
{
if (!response.Success)
fault = Fault.FromException(
new InvalidOperationException(response.ErrorCode.ToString()));
},
delegate(Fault f)
{
fault = f;
});
if (leftTargetEncoderDegrees != 0 && rightTargetEncoderDegrees != 0)
{
// Wait for the target encoders to either be cancelled or reach its target.
Activate(Arbiter.MultiplePortReceive<bool>(false, _targetEncoderReachedPort,
delegate(bool[] finished)
{
HandleDriveResponse(driveDistance, finished[LEFT] && finished[RIGHT], null);
}));
}
else if (leftTargetEncoderDegrees != 0 || rightTargetEncoderDegrees != 0)
{
int ix = (leftTargetEncoderDegrees != 0) ? LEFT : RIGHT;
// Wait for the target encoder to either be cancelled or reach its target.
Activate(Arbiter.Receive<bool>(false, _targetEncoderReachedPort[ix],
delegate(bool finished)
{
HandleDriveResponse(driveDistance, finished, null);
}));
}
else
{
bool success = (fault == null);
HandleDriveResponse(driveDistance, success, fault);
}
}
finally
{
// Wait one time for the next InternalDrivePower command
Activate(Arbiter.ReceiveWithIterator(false, _internalDrivePowerPort, InternalDrivePowerHandler));
}
yield break;
}