public virtual IEnumerator<ITask> SetMotorRotationHandler(SetMotorRotation setMotorRotation)
{
if (!ValidateConnection(_state, setMotorRotation.ResponsePort))
yield break;
_internalMotorRotationPort.Post(setMotorRotation);
yield break;
}
public virtual IEnumerator<ITask> RotateForDurationHandler(RotateForDuration rotateForDuration)
{
if (!ValidateConnection(_state, rotateForDuration.ResponsePort))
yield break;
// Send a notification that the power has been set.
SendNotification<RotateForDuration>(_subMgrPort, rotateForDuration);
SetMotorRotation setMotorRotation = new SetMotorRotation(new SetMotorRotationRequest(rotateForDuration.Body.TargetPower));
_internalMotorRotationPort.Post(setMotorRotation);
yield return Timeout(rotateForDuration.Body.StopAfterMs);
Arbiter.Receive(false, TimeoutPort(new TimeSpan((long)(10000.0 * rotateForDuration.Body.StopAfterMs))), EmptyHandler<DateTime>);
setMotorRotation = new SetMotorRotation(new SetMotorRotationRequest(0.0), rotateForDuration.ResponsePort);
setMotorRotation.Body.StopState = rotateForDuration.Body.StopState;
_internalMotorRotationPort.Post(setMotorRotation);
yield break;
}
public virtual IEnumerator<ITask> GenericSetMotorPowerHandler(pxmotor.SetMotorPower genericMotorPower)
{
if (!ValidateConnection(_state, genericMotorPower.ResponsePort))
yield break;
SetMotorRotation setMotorRotation = new SetMotorRotation(new SetMotorRotationRequest(genericMotorPower.Body.TargetPower));
_internalMotorRotationPort.Post(setMotorRotation);
yield return Arbiter.Choice(setMotorRotation.ResponsePort,
delegate(DefaultUpdateResponseType response)
{
SendNotification<pxmotor.SetMotorPower>(_subMgrPort, genericMotorPower);
genericMotorPower.ResponsePort.Post(response);
},
delegate(Fault fault)
{
genericMotorPower.ResponsePort.Post(fault);
});
yield break;
}
/// <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));
}
}
public virtual IEnumerator<ITask> AllStopHandler(AllStop stop)
{
if (!ValidateConnection(_state, stop.ResponsePort))
yield break;
SetMotorRotation setMotorRotation = new SetMotorRotation(new SetMotorRotationRequest(0.0));
setMotorRotation.Body.StopState = stop.Body.StopState;
_internalMotorRotationPort.Post(setMotorRotation);
yield return Arbiter.Choice(setMotorRotation.ResponsePort,
delegate(DefaultUpdateResponseType response)
{
stop.ResponsePort.Post(response);
},
delegate(Fault fault)
{
stop.ResponsePort.Post(fault);
});
yield break;
}
