private void cancelCurrentOperation()
{
switch (_internalPendingDriveOperation)
{
case drive.DriveRequestOperation.DriveDistance:
{
#if TRACELOG
Tracer.Trace("cancelCurrentOperation() - DriveDistance - completionPort.ItemCount=" + completionPort.ItemCount);
#endif
if (pendingDriveDistance != null)
{
EncoderTicksEnabled = false;
// send notification to subscription manager
pendingDriveDistance.Body.DriveDistanceStage = drive.DriveStage.Canceled;
double distanceTraveled = pendingDriveDistance.Body.Distance / 2.0d; // TODO: need to compute real distance traveled
pendingDriveDistance.Body.Distance = distanceTraveled;
SendNotification<drive.DriveDistance>(_subMgrPort, pendingDriveDistance);
pendingDriveDistance = null;
}
}
break;
case drive.DriveRequestOperation.RotateDegrees:
{
#if TRACELOG
Tracer.Trace("cancelCurrentOperation() - RotateDegrees - completionPort.ItemCount=" + completionPort.ItemCount);
#endif
if (pendingRotateDegrees != null)
{
EncoderTicksEnabled = false;
// send notification to subscription manager
pendingRotateDegrees.Body.RotateDegreesStage = drive.DriveStage.Canceled;
double angleRotated = pendingRotateDegrees.Body.Degrees / 2.0d; // TODO: need to compute real angle rotated
pendingRotateDegrees.Body.Degrees = angleRotated;
SendNotification<drive.RotateDegrees>(_subMgrPort, pendingRotateDegrees);
pendingRotateDegrees = null;
}
}
break;
case drive.DriveRequestOperation.NotSpecified:
break;
default:
Tracer.Trace("Warning: cancelCurrentOperation() - no pending wait-type operation to cancel - current operation = " + _internalPendingDriveOperation);
break;
}
_internalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified;
}
/// <summary>
/// Internal rotate degrees handler
/// </summary>
/// <param name="rotateDegrees"></param>
/// <returns></returns>
public virtual IEnumerator<ITask> InternalRotateDegreesHandler(drive.RotateDegrees rotateDegrees)
{
#if TRACELOG
Tracer.Trace("InternalRotateDegreesHandler() - DriveStage." + rotateDegrees.Body.RotateDegreesStage);
#endif
switch (rotateDegrees.Body.RotateDegreesStage)
{
case drive.DriveStage.InitialRequest:
cancelCurrentOperation();
pendingRotateDegrees = rotateDegrees;
// _state.InternalPendingDriveOperation = drive.DriveRequestOperation.RotateDegrees; - not available in Proxy
_internalPendingDriveOperation = drive.DriveRequestOperation.RotateDegrees;
SpawnIterator<double, double>(rotateDegrees.Body.Degrees, rotateDegrees.Body.Power, RotateUntilDegrees);
break;
case drive.DriveStage.Started:
SendNotification<drive.RotateDegrees>(_subMgrPort, rotateDegrees.Body);
break;
case drive.DriveStage.Completed:
pendingRotateDegrees = null;
// _state.InternalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified; - not available in Proxy
_internalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified;
SendNotification<drive.RotateDegrees>(_subMgrPort, rotateDegrees.Body);
break;
case drive.DriveStage.Canceled:
pendingRotateDegrees = null;
// _state.InternalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified; - not available in Proxy
_internalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified;
SendNotification<drive.RotateDegrees>(_subMgrPort, rotateDegrees.Body);
break;
}
yield break;
}
/// <summary>
/// Service Startup Handler
/// </summary>
protected override void Start()
{
LogInfo("TrackRoamerDriveService:: Start() ");
InitState();
_internalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified;
// send configuration commands to partner services
SpawnIterator(ConfigureDrive);
_state.TimeStamp = DateTime.Now;
//needed for HttpPost
_httpUtilities = DsspHttpUtilitiesService.Create(Environment);
base.Start();
// Interleave to manage internal drive operations (driveDistance and RotateDegrees)
Activate(
new Interleave(
new ExclusiveReceiverGroup(
Arbiter.ReceiveWithIterator(true, this.encodersPollingPort, this.PollEncoders),
Arbiter.ReceiveWithIteratorFromPortSet<drive.DriveDistance>(true, _internalDriveOperationsPort, InternalDriveDistanceHandler),
Arbiter.ReceiveWithIteratorFromPortSet<drive.RotateDegrees>(true, _internalDriveOperationsPort, InternalRotateDegreesHandler)
),
new ConcurrentReceiverGroup())
);
}
/// <summary>
/// Internal drive distance operation handler
/// </summary>
/// <param name="driveDistance"></param>
/// <returns></returns>
public virtual IEnumerator<ITask> InternalDriveDistanceHandler(drive.DriveDistance driveDistance)
{
#if TRACELOG
Tracer.Trace("InternalDriveDistanceHandler() - DriveStage." + driveDistance.Body.DriveDistanceStage);
#endif
switch (driveDistance.Body.DriveDistanceStage)
{
case drive.DriveStage.InitialRequest:
cancelCurrentOperation();
pendingDriveDistance = driveDistance; // we will need it for possible cancelation
// _state.InternalPendingDriveOperation = drive.DriveRequestOperation.DriveDistance; - not available in Proxy
_internalPendingDriveOperation = drive.DriveRequestOperation.DriveDistance;
SpawnIterator<double, double>(driveDistance.Body.Distance, driveDistance.Body.Power, DriveUntilDistance);
break;
case drive.DriveStage.Started:
SendNotification<drive.DriveDistance>(_subMgrPort, driveDistance.Body);
break;
case drive.DriveStage.Completed:
pendingDriveDistance = null;
// _state.InternalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified; - not available in Proxy
_internalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified;
SendNotification<drive.DriveDistance>(_subMgrPort, driveDistance.Body);
break;
case drive.DriveStage.Canceled:
pendingDriveDistance = null;
// _state.InternalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified; - not available in Proxy
_internalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified;
SendNotification<drive.DriveDistance>(_subMgrPort, driveDistance.Body);
break;
}
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;
}
}
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;
}
/// <summary>
/// Initialize and validate startup DriveState
/// </summary>
private void InitializeState()
{
if (_state == null)
_state = new DriveState();
if (_state.LeftWheel == null)
_state.LeftWheel = new WheelConfiguration();
if (_state.RightWheel == null)
_state.RightWheel = new WheelConfiguration();
if (_state.PollingFrequencyMs == 0)
_state.PollingFrequencyMs = Contract.DefaultPollingFrequencyMs;
if (_state.LeftWheel.WheelDiameter == 0.0)
_state.LeftWheel.WheelDiameter = 0.055;
if (_state.RightWheel.WheelDiameter == 0.0)
_state.RightWheel.WheelDiameter = 0.055;
// Always initialize the runtime statistics when we start.
_state.RuntimeStatistics = new RuntimeStatistics();
_targetEncoderReachedPort[LEFT] = new Port<bool>();
_targetEncoderReachedPort[RIGHT] = new Port<bool>();
_state.Connected = false;
_genericState.IsEnabled = true;
_internalPendingDriveOperation = pxdrive.DriveRequestOperation.NotSpecified;
}
