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;
}
///////////////////////////////////////////////////////////////////
// __use__.snippet.snippet.noop - [(__use__.snippet.snippet.expr1 - __use__.snippet.snippet.join)(__use__.snippet.snippet.expr0 - &__use__.snippet.snippet.join)(__use__.snippet.snippet.expr - &__use__.snippet.snippet.join)] - __use__.snippet.call - __use__.snippet.call.iftype
///////////////////////////////////////////////////////////////////
public IEnumerator <ccr.ITask> RunHandler(DriveRequest message, dssp.DsspResponsePort <DriveResponse> responsePort)
{
_responsePort = responsePort;
Increment();
drive.DriveDistanceRequest request = new drive.DriveDistanceRequest();
request.DriveDistanceStage = Microsoft.Robotics.Services.Drive.Proxy.DriveStage.InitialRequest;
request.Power = message.Power;
request.Distance = message.Distance;
Increment();
Activate(
ccr.Arbiter.Choice(
GenericDifferentialDrivePort.DriveDistance(request),
OnDriveDistanceSuccess,
delegate(soap.Fault fault)
{
base.FaultHandler(fault, @"GenericDifferentialDrivePort.DriveDistance(request)");
Decrement();
}
)
);
Decrement();
yield return(WaitUntilComplete());
}
/// <summary>
/// Moves the drive forward for the specified distance.
/// </summary>
/// <param name="step">distance in mm</param>
/// <returns>response port</returns>
PortSet <DefaultUpdateResponseType, Fault> Translate(int step)
{
if (_state.DriveState == null || !_state.DriveState.IsEnabled)
{
EnableMotor();
}
drive.DriveDistanceRequest request = new drive.DriveDistanceRequest();
request.Distance = (double)step / 1000.0; // millimeters to meters
return(_drivePort.DriveDistance(request));
}
/// <summary>
/// Handle Motion Commands
/// </summary>
/// <param name="onCommand">The motion command</param>
/// <returns>An Iterator</returns>
private IEnumerator <ITask> OnMotionCommandHandler(OnMotionCommand onCommand)
{
if (onCommand.DashboardForm == this.dashboardForm && this.drivePort != null)
{
switch (onCommand.Command)
{
case MOTIONCOMMANDS.Rotate:
var rotRequest = new drive.RotateDegreesRequest
{
Degrees = onCommand.Parameter, Power = onCommand.Power * MotorPowerSaleFactor
};
yield return(Arbiter.Choice(this.drivePort.RotateDegrees(rotRequest), EmptyHandler, LogError));
break;
case MOTIONCOMMANDS.Translate:
var transRequest = new drive.DriveDistanceRequest
{
Distance = onCommand.Parameter, Power = onCommand.Power * MotorPowerSaleFactor
};
yield return(Arbiter.Choice(this.drivePort.DriveDistance(transRequest), EmptyHandler, LogError));
break;
case MOTIONCOMMANDS.Enable:
var request = new drive.EnableDriveRequest {
Enable = true
};
yield return(Arbiter.Choice(this.drivePort.EnableDrive(request), EmptyHandler, LogError));
break;
default:
LogInfo("Requesting EStop");
var stopRequest = new drive.AllStopRequest();
yield return(Arbiter.Choice(this.drivePort.AllStop(stopRequest), EmptyHandler, LogError));
break;
}
}
yield break;
}
/// <summary>
/// Moves the drive forward for the specified distance.
/// </summary>
/// <param name="step">distance in mm</param>
/// <returns>response port</returns>
PortSet<DefaultUpdateResponseType, Fault> Translate(int step)
{
if (_state.DriveState == null || !_state.DriveState.IsEnabled)
{
EnableMotor();
}
drive.DriveDistanceRequest request = new drive.DriveDistanceRequest();
request.Distance = (double)step / 1000.0; // millimeters to meters
return _drivePort.DriveDistance(request);
}
IEnumerator<ITask> OnTranslateHandler(OnTranslate onTranslate)
{
if (onTranslate.DriveControl == _driveControl && _drivePort != null)
{
drive.DriveDistanceRequest request = new drive.DriveDistanceRequest();
request.Distance = onTranslate.Distance;
request.Power = (double)onTranslate.Power * MOTOR_POWER_SCALE_FACTOR;
yield return Arbiter.Choice(
_drivePort.DriveDistance(request),
delegate(DefaultUpdateResponseType response) { },
delegate(Fault f)
{
LogError(f);
}
);
}
}
/// <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;
}
}
