///////////////////////////////////////////////////////////////////
// __use__.snippet0.snippet.noop - [(__use__.snippet0.snippet.expr1 - __use__.snippet0.snippet.join)(__use__.snippet0.snippet.expr0 - &__use__.snippet0.snippet.join)(__use__.snippet0.snippet.expr - &__use__.snippet0.snippet.join)] - __use__.snippet0.call - __use__.snippet0.call.iftype
///////////////////////////////////////////////////////////////////
public IEnumerator <ccr.ITask> RunHandler(RotateRequest message, dssp.DsspResponsePort <RotateResponse> responsePort)
{
_responsePort = responsePort;
Increment();
drive.RotateDegreesRequest request = new drive.RotateDegreesRequest();
request.RotateDegreesStage = Microsoft.Robotics.Services.Drive.Proxy.DriveStage.InitialRequest;
request.Power = message.Power;
request.Degrees = message.Degrees;
Increment();
Activate(
ccr.Arbiter.Choice(
GenericDifferentialDrivePort.RotateDegrees(request),
OnRotateDegreesSuccess,
delegate(soap.Fault fault)
{
base.FaultHandler(fault, @"GenericDifferentialDrivePort.RotateDegrees(request)");
Decrement();
}
)
);
Decrement();
yield return(WaitUntilComplete());
}
/// <summary>
/// Turns the drive relative to its current heading.
/// </summary>
/// <param name="angle">angle in degrees</param>
/// <returns>response port</returns>
PortSet <DefaultUpdateResponseType, Fault> Turn(int angle)
{
if (_state.DriveState == null || !_state.DriveState.IsEnabled)
{
EnableMotor();
}
drive.RotateDegreesRequest request = new drive.RotateDegreesRequest();
request.Degrees = (double)(-angle);
return(_drivePort.RotateDegrees(request));
}
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>
/// 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>
/// Turns the drive relative to its current heading.
/// </summary>
/// <param name="angle">angle in degrees</param>
/// <returns>response port</returns>
PortSet<DefaultUpdateResponseType, Fault> Turn(int angle)
{
if (_state.DriveState == null || !_state.DriveState.IsEnabled)
{
EnableMotor();
}
drive.RotateDegreesRequest request = new drive.RotateDegreesRequest();
request.Degrees = (double)(-angle);
return _drivePort.RotateDegrees(request);
}
IEnumerator<ITask> OnRotateHandler(OnRotate onRotate)
{
if (onRotate.DriveControl == _driveControl && _drivePort != null)
{
drive.RotateDegreesRequest request = new drive.RotateDegreesRequest();
request.Degrees = onRotate.Angle;
request.Power = (double)onRotate.Power * MOTOR_POWER_SCALE_FACTOR;
yield return Arbiter.Choice(
_drivePort.RotateDegrees(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;
}
}
