/// <summary>
/// Internal drive distance operation handler
/// </summary>
/// <param name="driveDistance"></param>
/// <returns></returns>
public virtual IEnumerator <ITask> InternalDriveDistanceHandler(drive.DriveDistance driveDistance)
{
switch (driveDistance.Body.DriveDistanceStage)
{
case drive.DriveStage.InitialRequest:
_state.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:
_state.InternalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified;
SendNotification <drive.DriveDistance>(_subMgrPort, driveDistance.Body);
break;
case drive.DriveStage.Canceled:
_state.InternalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified;
SendNotification <drive.DriveDistance>(_subMgrPort, driveDistance.Body);
break;
}
yield break;
}
public IEnumerator <ITask> DriveDistanceHandler(diffdrive.DriveDistance driveDistance)
{
if (_entity == null)
{
throw new InvalidOperationException("Simulation entity not registered with service");
}
if (!_state.IsEnabled)
{
driveDistance.ResponsePort.Post(Fault.FromException(new Exception("Drive is not enabled.")));
LogError("DriveDistance request to disabled drive.");
yield break;
}
if ((driveDistance.Body.Power > 1.0f) || (driveDistance.Body.Power < -1.0f))
{
// invalid drive power
driveDistance.ResponsePort.Post(Fault.FromException(new Exception("Invalid Power parameter.")));
LogError("Invalid Power parameter in DriveDistanceHandler.");
yield break;
}
_state.DriveDistanceStage = driveDistance.Body.DriveDistanceStage;
if (driveDistance.Body.DriveDistanceStage == 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.DriveDistanceRequest req = new diffdrive.DriveDistanceRequest(0, 0);
switch (result)
{
case simengine.OperationResult.Error:
req.DriveDistanceStage = diffdrive.DriveStage.Canceled;
break;
case simengine.OperationResult.Canceled:
req.DriveDistanceStage = diffdrive.DriveStage.Canceled;
break;
case simengine.OperationResult.Completed:
req.DriveDistanceStage = diffdrive.DriveStage.Completed;
break;
}
_mainPort.Post(new diffdrive.DriveDistance(req));
}));
_entity.DriveDistance((float)driveDistance.Body.Distance, (float)driveDistance.Body.Power, entityResponse);
diffdrive.DriveDistanceRequest req2 = new diffdrive.DriveDistanceRequest(0, 0);
req2.DriveDistanceStage = diffdrive.DriveStage.Started;
_mainPort.Post(new diffdrive.DriveDistance(req2));
}
else
{
base.SendNotification(_subMgrPort, driveDistance);
}
driveDistance.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
/// <summary>
/// DriveDistanceUpdateHandler - Posts a message on Canceled or Completed
/// </summary>
/// <param name="distance"></param>
/// <returns></returns>
IEnumerator <ITask> DriveDistanceUpdateHandler(drive.DriveDistance distance)
{
if (distance.Body.DriveDistanceStage == drive.DriveStage.Canceled)
{
completionPort.Post(distance.Body.DriveDistanceStage);
}
if (distance.Body.DriveDistanceStage == drive.DriveStage.Completed)
{
completionPort.Post(distance.Body.DriveDistanceStage);
}
yield break;
}
public IEnumerator <ITask> DriveDistanceHandler(drive.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>
/// DriveDistanceUpdateHandler - Posts a message on Canceled or Complete
/// </summary>
/// <param name="distance"></param>
/// <returns></returns>
IEnumerator <ITask> DriveDistanceUpdateHandler(drive.DriveDistance distance)
{
// This code could be collapsed, but this structure makes it
// clear and also easy to change
if (distance.Body.DriveDistanceStage == drive.DriveStage.Canceled)
{
LogInfo(LogGroups.Console, "Drive Canceled");
completionPort.Post(distance.Body.DriveDistanceStage);
}
if (distance.Body.DriveDistanceStage == drive.DriveStage.Completed)
{
LogInfo(LogGroups.Console, "Drive Complete");
completionPort.Post(distance.Body.DriveDistanceStage);
}
yield break;
}
/// <summary>
/// drives a specified number of meters
/// </summary>
IEnumerator <ITask> DriveUntilDistance(double distance, double power)
{
LogInfo("=============== TrackRoamerDriveService:: DriveUntilDistance(distance=" + distance + " meters, 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); }
));
//compute tick to stop at
stopLeftWheelAt = (int)Math.Round(Math.Abs(distance) / (2.0 * 3.14159 * _state.LeftWheel.Radius / _state.LeftWheel.EncoderState.TicksPerRevolution));
stopRightWheelAt = (int)Math.Round(Math.Abs(distance) / (2.0 * 3.14159 * _state.RightWheel.Radius / _state.RightWheel.EncoderState.TicksPerRevolution));
_leftEncoderTickEnabled = true;
_rightEncoderTickEnabled = true;
//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 = drive.DriveStage.Started;
drive.DriveDistance driveUpdate = new drive.DriveDistance();
driveUpdate.Body.DriveDistanceStage = drive.DriveStage.Started;
_internalDriveOperationsPort.Post(driveUpdate);
motor.SetMotorPower leftPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest()
{
TargetPower = Pow
});
leftPower.ResponsePort = responsePort;
_leftMotorPort.Post(leftPower);
motor.SetMotorPower rightPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest()
{
TargetPower = Pow
});
rightPower.ResponsePort = responsePort;
_rightMotorPort.Post(rightPower);
LogInfo("=============== TrackRoamerDriveService:: DriveUntilDistance() start moving: distance=" + distance + " meters");
LogInfo("=============== TrackRoamerDriveService:: DriveUntilDistance() will stop wheel 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:: DriveUntilDistance() calling WaitForCompletion() - waiting for the first side to complete...");
yield return(WaitForCompletion());
LogInfo("=============== TrackRoamerDriveService:: DriveUntilDistance() calling WaitForCompletion() - other side should complete too...");
yield return(WaitForCompletion());
LogInfo("=============== TrackRoamerDriveService:: DriveUntilDistance() - both sides completed, send notification of driveDistance complete to subscription manager");
// send notification of driveDistance complete to subscription manager
driveUpdate.Body.DriveDistanceStage = drive.DriveStage.Completed;
_internalDriveOperationsPort.Post(driveUpdate);
_state.DriveDistanceStage = drive.DriveStage.Completed;
}
public IEnumerator <ITask> DriveDistanceHandler(drive.DriveDistance driveDistance)
{
LogError("Drive distance is not implemented");
yield break;
}
public virtual IEnumerator <ITask> DriveDistanceHandler(drive.DriveDistance update)
{
// TODO: Implement DriveDistance operation here.
throw new NotImplementedException("DriveDistance is not Implemented.");
}
/// <summary>
/// drives a specified number of meters
/// </summary>
IEnumerator<ITask> DriveUntilDistance(double distance, double power)
{
#if TRACELOG
Tracer.Trace("=============== TrackRoamerDriveService:: DriveUntilDistance(distance=" + distance + " meters, 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); }
));
//compute tick to stop at
stopLeftWheelAt = (int)Math.Round(Math.Abs(distance) / (2.0 * 3.14159 * _state.LeftWheel.Radius / _state.LeftWheel.EncoderState.TicksPerRevolution));
stopRightWheelAt = (int)Math.Round(Math.Abs(distance) / (2.0 * 3.14159 * _state.RightWheel.Radius / _state.RightWheel.EncoderState.TicksPerRevolution));
EncoderTicksEnabled = true;
pollEncoderState(); // get starting encoder state
//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 = drive.DriveStage.Started;
drive.DriveDistance driveUpdate = new drive.DriveDistance();
driveUpdate.Body.DriveDistanceStage = drive.DriveStage.Started;
#if TRACELOG
Tracer.Trace("++++++++++++++++++ DRIVE: DriveUntilDistance() DriveStage.Started");
#endif
_internalDriveOperationsPort.Post(driveUpdate);
motor.SetMotorPower leftPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = Pow });
leftPower.ResponsePort = responsePort;
_leftMotorPort.Post(leftPower);
motor.SetMotorPower rightPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = Pow });
rightPower.ResponsePort = responsePort;
_rightMotorPort.Post(rightPower);
#if TRACELOG
Tracer.Trace("=============== TrackRoamerDriveService:: DriveUntilDistance() start moving: distance=" + distance + " meters");
Tracer.Trace("=============== TrackRoamerDriveService:: DriveUntilDistance() 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:: DriveUntilDistance() calling DriveWaitForCompletionDual() - waiting for both sides to complete...");
#endif
yield return DriveWaitForCompletionDual();
// send notification of driveDistance complete to subscription manager
driveUpdate.Body.DriveDistanceStage = drive.DriveStage.Completed;
#if TRACELOG
Tracer.Trace("++++++++++++++++++ DRIVE: DriveUntilDistance() DriveStage.Completed");
#endif
_internalDriveOperationsPort.Post(driveUpdate);
_state.DriveDistanceStage = drive.DriveStage.Completed;
}
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 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> 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;
}
