public IEnumerator<ITask> DriveDistanceHandler(DriveDistance driveDistance)
{
// If configuration is invalid, an InvalidException is thrown.
ValidateDriveConfiguration(true);
_state.TimeStamp = DateTime.Now;
// send immediate response
driveDistance.ResponsePort.Post(DefaultUpdateResponseType.Instance);
// post request to internal port.
_internalDriveOperationsPort.Post(driveDistance);
yield break;
}
/// <summary>
/// drives a specified number of meters
/// </summary>
IEnumerator<ITask> DriveUntilDistance(double distance, double power)
{
//reset encoders
encoder.Reset Lreset = new encoder.Reset();
_leftEncoderPort.Post(Lreset);
yield return (Arbiter.Choice(Lreset.ResponsePort,
delegate(DefaultUpdateResponseType response) { },
delegate(Fault fault) { LogError(fault); }
));
encoder.Reset Rreset = new encoder.Reset();
_rightEncoderPort.Post(Rreset);
yield return (Arbiter.Choice(Rreset.ResponsePort,
delegate(DefaultUpdateResponseType response) { },
delegate(Fault fault) { LogError(fault); }
));
//compute tick to stop at
int stopLeftWheelAt = (int)Math.Round(Math.Abs(distance) / (2.0 * 3.14159 * _state.LeftWheel.Radius / _state.LeftWheel.EncoderState.TicksPerRevolution));
int stopRightWheelAt = (int)Math.Round(Math.Abs(distance) / (2.0 * 3.14159 * _state.RightWheel.Radius / _state.RightWheel.EncoderState.TicksPerRevolution));
// Subscribe to the encoders on internal ports
encoder.EncoderOperations leftNotificationPort = new encoder.EncoderOperations();
encoder.EncoderOperations rightNotificationPort = new encoder.EncoderOperations();
encoder.Subscribe op = new encoder.Subscribe();
op.Body = new SubscribeRequestType();
op.NotificationPort = leftNotificationPort;
_leftEncoderPort.Post(op);
yield return (Arbiter.Choice(op.ResponsePort,
delegate(SubscribeResponseType response)
{
//subscription was successful, start listening for encoder replace messages
Activate(Arbiter.Receive<encoder.UpdateTickCount>(false, leftNotificationPort,
delegate(encoder.UpdateTickCount update)
{
StopMotorWithEncoderHandler(leftNotificationPort, update, _leftMotorPort, stopLeftWheelAt);
}));
},
delegate(Fault fault) { LogError(fault); }
));
encoder.Subscribe op2 = new encoder.Subscribe();
op2.Body = new SubscribeRequestType();
op2.NotificationPort = rightNotificationPort;
_leftEncoderPort.Post(op2);
yield return (Arbiter.Choice(op2.ResponsePort,
delegate(SubscribeResponseType response)
{
//subscription was successful, start listening for encoder replace messages
Activate(Arbiter.Receive<encoder.UpdateTickCount>(false, rightNotificationPort,
delegate(encoder.UpdateTickCount update)
{
StopMotorWithEncoderHandler(rightNotificationPort, update, _rightMotorPort, stopRightWheelAt);
}
));
},
delegate(Fault fault) { LogError(fault); }
));
//start moving
double Pow;
if (distance > 0)
Pow = power;
else
Pow = -power;
PortSet<DefaultUpdateResponseType, Fault> responsePort = new PortSet<DefaultUpdateResponseType, Fault>();
// send notification of driveDistance start to subscription manager
_state.DriveDistanceStage = DriveStage.Started;
DriveDistance driveUpdate = new DriveDistance();
driveUpdate.Body.DriveDistanceStage = DriveStage.Started;
_internalDriveOperationsPort.Post(driveUpdate);
Motor.SetMotorPower leftPower = new Motor.SetMotorPower(Pow);
leftPower.ResponsePort = responsePort;
_leftMotorPort.Post(leftPower);
Motor.SetMotorPower rightPower = new Motor.SetMotorPower(Pow);
rightPower.ResponsePort = responsePort;
_rightMotorPort.Post(rightPower);
Activate(Arbiter.MultipleItemReceive<DefaultUpdateResponseType, Fault>(responsePort, 2,
delegate(ICollection<DefaultUpdateResponseType> successList, ICollection<Fault> failureList)
{
foreach (Fault fault in failureList)
{
LogError(fault);
}
}
));
// send notification of driveDistance complete to subscription manager
driveUpdate.Body.DriveDistanceStage = DriveStage.Completed;
_internalDriveOperationsPort.Post(driveUpdate);
_state.DriveDistanceStage = DriveStage.Completed;
}
public virtual IEnumerator<ITask> MainAllStopHandler(motor.AllStop allStop)
{
DriveDistance drive = new DriveDistance(new SetDriveRequest());
drive.Body.LeftPower = 0.0;
drive.Body.RightPower = 0.0;
drive.Body.LeftStopAtRotationDegrees = 0;
drive.Body.RightStopAtRotationDegrees = 0;
drive.Body.StopState = allStop.Body.StopState;
drive.ResponsePort = allStop.ResponsePort;
drive.Body.DriveRequestOperation = pxdrive.DriveRequestOperation.AllStop;
_internalDrivePowerPort.Post(drive);
allStop.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
public virtual IEnumerator<ITask> MainDriveDistanceHandler(DriveDistance driveDistance)
{
_internalDrivePowerPort.Post(driveDistance);
driveDistance.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
/// <summary>
/// Notified subscribers a success or fault to completed Drive request.
/// </summary>
/// <param name="driveDistance"></param>
/// <param name="success"></param>
/// <param name="fault"></param>
public void HandleDriveResponseForGenericOperationsNotifications(DriveDistance driveDistance, bool success, Fault fault)
{
if (fault == null)
{
if (driveDistance.Body.isGenericOperation == true)
{
//notify subscribers of generic drive distance -- complete
switch (driveDistance.Body.DriveRequestOperation)
{
case pxdrive.DriveRequestOperation.DriveDistance:
pxdrive.DriveDistanceRequest driveDistanceRequest = new pxdrive.DriveDistanceRequest();
driveDistanceRequest.DriveDistanceStage = pxdrive.DriveStage.Completed;
pxdrive.DriveDistance driveDistanceUpdate = new pxdrive.DriveDistance(driveDistanceRequest);
SendNotification<pxdrive.DriveDistance>(_genericSubMgrPort, driveDistanceUpdate);
break;
case pxdrive.DriveRequestOperation.RotateDegrees:
pxdrive.RotateDegreesRequest rotateDegreesRequest = new pxdrive.RotateDegreesRequest();
rotateDegreesRequest.RotateDegreesStage = pxdrive.DriveStage.Completed;
pxdrive.RotateDegrees rotateDegreesUpdate = new pxdrive.RotateDegrees(rotateDegreesRequest);
SendNotification<pxdrive.RotateDegrees>(_genericSubMgrPort, rotateDegreesUpdate);
break;
}
}
else
{
// Operation canceled.
driveDistance.Body.DriveDistanceStage = pxdrive.DriveStage.Canceled;
SendNotification<pxdrive.SetDrivePower>(_genericSubMgrPort, driveDistance.Body);
}
_internalPendingDriveOperation = pxdrive.DriveRequestOperation.NotSpecified;
}
}
/// <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;
}
public virtual IEnumerator<ITask> GenericSetDriveSpeedHandler(pxdrive.SetDriveSpeed driveSpeed)
{
// set back response immediately or fault if drive is not enabled.
ValidateDriveEnabledAndRespondHelper(driveSpeed.ResponsePort);
DriveDistance drive = new DriveDistance(new SetDriveRequest());
drive.Body.LeftPower = driveSpeed.Body.LeftWheelSpeed;
drive.Body.RightPower = driveSpeed.Body.RightWheelSpeed;
drive.Body.LeftStopAtRotationDegrees = 0;
drive.Body.RightStopAtRotationDegrees = 0;
drive.Body.StopState = MotorStopState.Default;
drive.Body.isGenericOperation = true;
drive.ResponsePort = driveSpeed.ResponsePort;
_internalDrivePowerPort.Post(drive);
yield break;
}
public virtual IEnumerator<ITask> GenericRotateDegreesHandler(pxdrive.RotateDegrees rotateDegrees)
{
if (_state.DistanceBetweenWheels <= 0)
{
rotateDegrees.ResponsePort.Post(Fault.FromException(new ArgumentOutOfRangeException("DistanceBetweenWheels must be specified in the Drive Configuration.")));
yield break;
}
// set back response immediately or fault if drive is not enabled.
ValidateDriveEnabledAndRespondHelper(rotateDegrees.ResponsePort);
// distance = circumference / 360 * degreesToTurn
double distance = Math.PI * _state.DistanceBetweenWheels * rotateDegrees.Body.Degrees / 360.0;
// axleRotationDegrees = distance (meters) / wheelCircumference (pi * diameter) * 360
long axleRotationDegrees = (long)Math.Round(Math.Abs(distance) / (Math.PI * _state.LeftWheel.WheelDiameter) * 360.0);
LogVerbose(LogGroups.Console, "RotateDegrees: Wheel Distance: " + distance.ToString() + " Axle Rotation Degrees: " + axleRotationDegrees.ToString());
double leftDirection = 1.0, rightDirection = 1.0;
if (rotateDegrees.Body.Degrees > 0)
leftDirection = -1.0;
else
rightDirection = -1.0;
DriveDistance drive = new DriveDistance(new SetDriveRequest());
drive.Body.LeftStopAtRotationDegrees = axleRotationDegrees;
drive.Body.RightStopAtRotationDegrees = axleRotationDegrees;
drive.Body.StopState = MotorStopState.Brake;
drive.Body.isGenericOperation = true;
drive.Body.DriveRequestOperation = pxdrive.DriveRequestOperation.RotateDegrees;
_internalPendingDriveOperation = pxdrive.DriveRequestOperation.RotateDegrees;
bool synchronized = false;
if (synchronized)
{
drive.Body.LeftPower = Math.Abs(rotateDegrees.Body.Power);
drive.Body.RightPower = Math.Abs(rotateDegrees.Body.Power);
}
else
{
drive.Body.LeftPower = rotateDegrees.Body.Power * leftDirection;
drive.Body.RightPower = rotateDegrees.Body.Power * rightDirection;
}
drive.ResponsePort = rotateDegrees.ResponsePort;
// notify subscribers of rotate degrees start
rotateDegrees.Body.RotateDegreesStage = pxdrive.DriveStage.Started;
pxdrive.RotateDegrees rotateDegreesUpdate = new pxdrive.RotateDegrees(rotateDegrees.Body);
SendNotification<pxdrive.RotateDegrees>(_genericSubMgrPort, rotateDegreesUpdate);
_internalDrivePowerPort.Post(drive);
yield break;
}
public virtual IEnumerator<ITask> GenericDriveDistanceHandler(pxdrive.DriveDistance driveDistance)
{
double distance = driveDistance.Body.Distance;
// set back response immediately or fault if drive is not enabled.
ValidateDriveEnabledAndRespondHelper(driveDistance.ResponsePort);
// rotations = distance (meters) / circumference (pi * diameter)
// degrees = rotations * 360
double stopLeftWheelAtDegrees = Math.Round(Math.Abs(distance) / (Math.PI * _state.LeftWheel.WheelDiameter) * 360.0);
double stopRightWheelAtDegrees = Math.Round(Math.Abs(distance) / (Math.PI * _state.RightWheel.WheelDiameter) * 360.0);
DriveDistance drive = new DriveDistance(new SetDriveRequest());
drive.Body.LeftPower = driveDistance.Body.Power;
drive.Body.RightPower = driveDistance.Body.Power;
drive.Body.LeftStopAtRotationDegrees = (long)stopLeftWheelAtDegrees;
drive.Body.RightStopAtRotationDegrees = (long)stopRightWheelAtDegrees;
drive.Body.StopState = MotorStopState.Brake;
drive.Body.isGenericOperation = true;
drive.Body.DriveRequestOperation = pxdrive.DriveRequestOperation.DriveDistance;
_internalPendingDriveOperation = pxdrive.DriveRequestOperation.DriveDistance;
drive.ResponsePort = driveDistance.ResponsePort;
// notify subscribers of drive distance start
driveDistance.Body.DriveDistanceStage = pxdrive.DriveStage.Started;
pxdrive.DriveDistance driveDistanceUpdate = new pxdrive.DriveDistance(driveDistance.Body);
SendNotification<pxdrive.DriveDistance>(_genericSubMgrPort, driveDistanceUpdate);
_internalDrivePowerPort.Post(drive);
yield break;
}
public virtual IEnumerator<ITask> GenericAllStopHandler(pxdrive.AllStop allStop)
{
DriveDistance drive = new DriveDistance(new SetDriveRequest());
drive.Body.LeftPower = 0.0;
drive.Body.RightPower = 0.0;
drive.Body.LeftStopAtRotationDegrees = 0;
drive.Body.RightStopAtRotationDegrees = 0;
drive.Body.StopState = MotorStopState.Brake;
drive.ResponsePort = allStop.ResponsePort;
drive.Body.DriveRequestOperation = pxdrive.DriveRequestOperation.AllStop;
_internalDrivePowerPort.Post(drive);
// disable drive
_genericState.IsEnabled = false;
pxdrive.EnableDrive disableDrive = new pxdrive.EnableDrive();
disableDrive.Body.Enable = _genericState.IsEnabled;
_genericState.TimeStamp = DateTime.Now;
_state.TimeStamp = _genericState.TimeStamp;
SendNotification<pxdrive.EnableDrive>(_genericSubMgrPort, disableDrive);
_drivePort.Post(disableDrive);
yield break;
}
/// <summary>
/// Post a success or fault to complete a DriveDistance request.
/// </summary>
/// <param name="driveDistance"></param>
/// <param name="success"></param>
/// <param name="fault"></param>
private void HandleDriveResponse(DriveDistance driveDistance, bool success, Fault fault)
{
if (success)
{
LogVerbose(LogGroups.Console, "InternalDrive Completed Successfully");
if (driveDistance.Body.isGenericOperation == true)
{
//notify subscribers of generic drive distance -- completed
HandleDriveResponseForGenericOperationsNotifications(driveDistance, success, fault);
}
else
{
//notify subscribers of specific drive distance -- completed
}
}
else
{
string msg;
if (fault == null)
{
msg = "The Drive operation was canceled due to a newer drive command.";
fault = Fault.FromException(new OperationCanceledException(msg));
}
else
{
msg = fault.ToString();
}
driveDistance.ResponsePort.Post(fault);
LogVerbose(LogGroups.Console, msg);
}
}
