/// <summary>
/// Internal rotate degrees handler
/// </summary>
/// <param name="rotateDegrees"></param>
/// <returns></returns>
public virtual IEnumerator <ITask> InternalRotateDegreesHandler(drive.RotateDegrees rotateDegrees)
{
switch (rotateDegrees.Body.RotateDegreesStage)
{
case drive.DriveStage.InitialRequest:
_state.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:
_state.InternalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified;
SendNotification <drive.RotateDegrees>(_subMgrPort, rotateDegrees.Body);
break;
case drive.DriveStage.Canceled:
_state.InternalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified;
SendNotification <drive.RotateDegrees>(_subMgrPort, rotateDegrees.Body);
break;
}
yield break;
}
/// <summary>
/// RotateDegreesUpdateHandler - Posts a message on Canceled or Completed
/// </summary>
/// <param name="rotate"></param>
/// <returns></returns>
IEnumerator <ITask> RotateDegreesUpdateHandler(drive.RotateDegrees rotate)
{
if (rotate.Body.RotateDegreesStage == drive.DriveStage.Canceled)
{
completionPort.Post(rotate.Body.RotateDegreesStage);
}
if (rotate.Body.RotateDegreesStage == drive.DriveStage.Completed)
{
completionPort.Post(rotate.Body.RotateDegreesStage);
}
yield break;
}
public IEnumerator <ITask> RotateHandler(diffdrive.RotateDegrees rotate)
{
if (_entity == null)
{
throw new InvalidOperationException("Simulation entity not registered with service");
}
if (!_state.IsEnabled)
{
rotate.ResponsePort.Post(Fault.FromException(new Exception("Drive is not enabled.")));
LogError("RotateDegrees request to disabled drive.");
yield break;
}
_state.RotateDegreesStage = rotate.Body.RotateDegreesStage;
if (rotate.Body.RotateDegreesStage == diffdrive.DriveStage.InitialRequest)
{
Port <simengine.OperationResult> entityResponse = new Port <simengine.OperationResult>();
Activate(Arbiter.Receive <simengine.OperationResult>(false, entityResponse, delegate(simengine.OperationResult result)
{
// post a message to ourselves indicating that the drive distance has completed
diffdrive.RotateDegreesRequest req = new diffdrive.RotateDegreesRequest(0, 0);
switch (result)
{
case simengine.OperationResult.Error:
req.RotateDegreesStage = diffdrive.DriveStage.Canceled;
break;
case simengine.OperationResult.Canceled:
req.RotateDegreesStage = diffdrive.DriveStage.Canceled;
break;
case simengine.OperationResult.Completed:
req.RotateDegreesStage = diffdrive.DriveStage.Completed;
break;
}
_mainPort.Post(new diffdrive.RotateDegrees(req));
}));
_entity.RotateDegrees((float)rotate.Body.Degrees, (float)rotate.Body.Power, entityResponse);
diffdrive.RotateDegreesRequest req2 = new diffdrive.RotateDegreesRequest(0, 0);
req2.RotateDegreesStage = diffdrive.DriveStage.Started;
_mainPort.Post(new diffdrive.RotateDegrees(req2));
}
else
{
base.SendNotification(_subMgrPort, rotate);
}
rotate.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
/// <summary>
/// RotateDegreesUpdateHandler - Posts a message on Canceled or Complete
/// </summary>
/// <param name="rotate"></param>
/// <returns></returns>
IEnumerator <ITask> RotateDegreesUpdateHandler(drive.RotateDegrees rotate)
{
if (rotate.Body.RotateDegreesStage == drive.DriveStage.Canceled)
{
LogInfo(LogGroups.Console, "Rotate Canceled");
completionPort.Post(rotate.Body.RotateDegreesStage);
}
if (rotate.Body.RotateDegreesStage == drive.DriveStage.Completed)
{
LogInfo(LogGroups.Console, "Rotate Complete");
completionPort.Post(rotate.Body.RotateDegreesStage);
}
yield break;
}
public IEnumerator <ITask> RotateDegreesHandler(drive.RotateDegrees rotateDegrees)
{
ValidateDriveConfiguration(true);
if (_state.DistanceBetweenWheels <= 0)
{
rotateDegrees.ResponsePort.Post(new Fault());
throw new InvalidOperationException("The wheel encoders are not properly configured");
}
else
{
_state.TimeStamp = DateTime.Now;
// send immediate response
rotateDegrees.ResponsePort.Post(DefaultUpdateResponseType.Instance);
// send immediate response
rotateDegrees.ResponsePort.Post(DefaultUpdateResponseType.Instance);
// post request to internal port.
_internalDriveOperationsPort.Post(rotateDegrees);
}
yield break;
}
/// <summary>
/// Rotate the the drive (positive degrees turn counterclockwise)
/// </summary>
/// <param name="degrees">(positive degrees turn counterclockwise)</param>
/// <param name="power">(-1.0 to 1.0)</param>
IEnumerator <ITask> RotateUntilDegrees(double degrees, double power)
{
LogInfo("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: RotateUntilDegrees(degrees=" + degrees + ", power=" + power + ")");
_leftEncoderTickEnabled = false;
_rightEncoderTickEnabled = false;
//reset encoders
encoder.Reset Lreset = new encoder.Reset();
_leftEncoderCmdPort.Post(Lreset);
yield return(Arbiter.Choice(Lreset.ResponsePort,
delegate(DefaultUpdateResponseType response) { },
delegate(Fault fault) { LogError(fault); }
));
encoder.Reset Rreset = new encoder.Reset();
_rightEncoderCmdPort.Post(Rreset);
yield return(Arbiter.Choice(Rreset.ResponsePort,
delegate(DefaultUpdateResponseType response) { },
delegate(Fault fault) { LogError(fault); }
));
double arcDistance = Math.Abs(degrees) * _state.DistanceBetweenWheels * Math.PI / 360.0d;
//compute tick to stop at
stopLeftWheelAt = (int)Math.Round(arcDistance / (2.0d * Math.PI * _state.LeftWheel.Radius / _state.LeftWheel.EncoderState.TicksPerRevolution));
stopRightWheelAt = (int)Math.Round(arcDistance / (2.0d * Math.PI * _state.RightWheel.Radius / _state.RightWheel.EncoderState.TicksPerRevolution));
_leftEncoderTickEnabled = true;
_rightEncoderTickEnabled = true;
//start moving
// start rotate operation
_state.RotateDegreesStage = drive.DriveStage.Started;
drive.RotateDegrees rotateUpdate = new drive.RotateDegrees();
rotateUpdate.Body.RotateDegreesStage = drive.DriveStage.Started;
_internalDriveOperationsPort.Post(rotateUpdate);
PortSet <DefaultUpdateResponseType, Fault> responsePort = new PortSet <DefaultUpdateResponseType, Fault>();
double rightPow;
double leftPow;
if (degrees > 0)
{
rightPow = power;
leftPow = -power;
}
else
{
rightPow = -power;
leftPow = power;
}
motor.SetMotorPower leftPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest()
{
TargetPower = leftPow
});
leftPower.ResponsePort = responsePort;
_leftMotorPort.Post(leftPower);
motor.SetMotorPower rightPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest()
{
TargetPower = rightPow
});
rightPower.ResponsePort = responsePort;
_rightMotorPort.Post(rightPower);
LogInfo("=============== TrackRoamerDriveService:: RotateUntilDegrees() start moving: degrees=" + degrees);
LogInfo("=============== TrackRoamerDriveService:: RotateUntilDegrees() will stop wheels at: Left=" + stopLeftWheelAt + " Right=" + stopRightWheelAt);
Activate(Arbiter.MultipleItemReceive <DefaultUpdateResponseType, Fault>(responsePort, 2,
delegate(ICollection <DefaultUpdateResponseType> successList, ICollection <Fault> failureList)
{
foreach (Fault fault in failureList)
{
LogError(fault);
}
}
));
LogInfo("=============== TrackRoamerDriveService:: RotateUntilDegrees() calling WaitForCompletion() - waiting for the first side to complete...");
yield return(WaitForCompletion());
LogInfo("=============== TrackRoamerDriveService:: RotateUntilDegrees() calling WaitForCompletion() - other side should complete too...");
yield return(WaitForCompletion());
LogInfo("=============== TrackRoamerDriveService:: RotateUntilDegrees() - both sides completed, send notification of RotateDegrees complete to subscription manager");
// send notification of RotateDegrees complete to subscription manager
rotateUpdate.Body.RotateDegreesStage = drive.DriveStage.Completed;
_internalDriveOperationsPort.Post(rotateUpdate);
_state.RotateDegreesStage = drive.DriveStage.Completed;
}
public IEnumerator <ITask> RotateDegreesHandler(drive.RotateDegrees rotateDegrees)
{
LogError("Rotate degrees is not implemented");
yield break;
}
public virtual IEnumerator <ITask> RotateDegreesHandler(drive.RotateDegrees update)
{
// TODO: Implement RotateDegrees operation here.
throw new NotImplementedException("RotateDegrees is not Implemented.");
}
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>
/// Rotate the the drive (positive degrees turn counterclockwise)
/// </summary>
/// <param name="degrees">(positive degrees turn counterclockwise)</param>
/// <param name="power">(-1.0 to 1.0)</param>
IEnumerator<ITask> RotateUntilDegrees(double degrees, double power)
{
#if TRACELOG
Tracer.Trace("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: RotateUntilDegrees(degrees=" + degrees + ", power=" + power + ")");
#endif
EncoderTicksEnabled = false;
//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); }
));
double arcDistance = Math.Abs(degrees) * _state.DistanceBetweenWheels * Math.PI / 360.0d;
//compute tick to stop at
stopLeftWheelAt = (int)Math.Round(arcDistance / (2.0d * Math.PI * _state.LeftWheel.Radius / _state.LeftWheel.EncoderState.TicksPerRevolution));
stopRightWheelAt = (int)Math.Round(arcDistance / (2.0d * Math.PI * _state.RightWheel.Radius / _state.RightWheel.EncoderState.TicksPerRevolution));
EncoderTicksEnabled = true;
pollEncoderState(); // get starting encoder state
//start moving
// start rotate operation
_state.RotateDegreesStage = drive.DriveStage.Started;
drive.RotateDegrees rotateUpdate = new drive.RotateDegrees();
rotateUpdate.Body.RotateDegreesStage = drive.DriveStage.Started;
#if TRACELOG
Tracer.Trace("++++++++++++++++++ DRIVE: RotateUntilDegrees() DriveStage.Started");
#endif
_internalDriveOperationsPort.Post(rotateUpdate);
PortSet<DefaultUpdateResponseType, Fault> responsePort = new PortSet<DefaultUpdateResponseType, Fault>();
double rightPow;
double leftPow;
if (degrees > 0)
{
rightPow = power;
leftPow = -power;
}
else
{
rightPow = -power;
leftPow = power;
}
motor.SetMotorPower leftPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = leftPow });
leftPower.ResponsePort = responsePort;
_leftMotorPort.Post(leftPower);
motor.SetMotorPower rightPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = rightPow });
rightPower.ResponsePort = responsePort;
_rightMotorPort.Post(rightPower);
#if TRACELOG
Tracer.Trace("=============== TrackRoamerDriveService:: RotateUntilDegrees() start moving: degrees=" + degrees);
Tracer.Trace("=============== TrackRoamerDriveService:: RotateUntilDegrees() will stop wheels at: Left=" + stopLeftWheelAt + " Right=" + stopRightWheelAt);
#endif
Activate(Arbiter.MultipleItemReceive<DefaultUpdateResponseType, Fault>(responsePort, 2,
delegate(ICollection<DefaultUpdateResponseType> successList, ICollection<Fault> failureList)
{
foreach (Fault fault in failureList)
{
LogError(fault);
}
}
));
#if TRACELOG
Tracer.Trace("=============== TrackRoamerDriveService:: RotateUntilDegrees() calling DriveWaitForCompletionDual() - waiting for both sides to complete...");
#endif
yield return DriveWaitForCompletionDual();
#if TRACELOG
Tracer.Trace("=============== TrackRoamerDriveService:: RotateUntilDegrees() - both sides completed, send notification of RotateDegrees complete to subscription manager");
#endif
// send notification of RotateDegrees complete to subscription manager
rotateUpdate.Body.RotateDegreesStage = drive.DriveStage.Completed;
#if TRACELOG
Tracer.Trace("++++++++++++++++++ DRIVE: RotateUntilDegrees() DriveStage.Completed");
#endif
_internalDriveOperationsPort.Post(rotateUpdate);
_state.RotateDegreesStage = drive.DriveStage.Completed;
}
/// <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;
}
