/// <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;
}
/// <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;
}
public IEnumerator<ITask> DriveDistanceHandler(drive.DriveDistance driveDistance)
{
LogError("Drive distance is not implemented");
yield break;
}
public virtual IEnumerator<ITask> GetHandler(drive.Get get)
{
get.ResponsePort.Post(_state);
yield break;
}
public IEnumerator<ITask> SetDriveSpeedHandler(drive.SetDriveSpeed setDriveSpeed)
{
LogError("Drive speed is not implemented");
yield break;
}
public IEnumerator<ITask> RotateDegreesHandler(drive.RotateDegrees rotateDegrees)
{
LogError("Rotate degrees is not implemented");
yield break;
}
public IEnumerator<ITask> AllStopHandler(drive.AllStop allStop)
{
drive.SetDrivePower zeroPower = new drive.SetDrivePower();
zeroPower.Body = new drive.SetDrivePowerRequest(0.0, 0.0);
zeroPower.ResponsePort = allStop.ResponsePort;
_mainPort.Post(zeroPower);
yield break;
}
public IEnumerator<ITask> SetDrivePowerHandler(drive.SetDrivePower setDrivePower)
{
if (setDrivePower.Body == null)
{
LogError("setDrivePower.Body == null");
setDrivePower.ResponsePort.Post(new Fault());
yield break;
}
double leftPower = setDrivePower.Body.LeftWheelPower;
double rightPower = setDrivePower.Body.RightWheelPower;
// Dump requests that come too fast.
// but let stop messages through
if (RequestPending > 1 && leftPower != 0 && rightPower != 0)
{
setDrivePower.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
RequestPending++;
BoundsCheck(leftPower);
BoundsCheck(rightPower);
//update state
_state.TimeStamp = DateTime.Now;
_state.LeftWheel.MotorState.CurrentPower = leftPower;
_state.RightWheel.MotorState.CurrentPower = rightPower;
//convert to native units
int leftPowerScaled = (int)Math.Round(leftPower * 100 + 100);
int rightPowerScaled = (int)Math.Round(rightPower * 100 + 100);
//send hardware specific motor data
brick.SetMotorsBody motordata = new brick.SetMotorsBody(leftPowerScaled, rightPowerScaled);
Activate(Arbiter.Choice(_scribblerPort.SetMotors(motordata),
delegate(DefaultUpdateResponseType status)
{
setDrivePower.ResponsePort.Post(DefaultUpdateResponseType.Instance);
RequestPending--;
},
delegate(Fault failure)
{
setDrivePower.ResponsePort.Post(failure);
RequestPending--;
}
));
yield break;
}
public IEnumerator<ITask> EnableDriveHandler(drive.EnableDrive enableDrive)
{
_state.IsEnabled = enableDrive.Body.Enable;
_state.TimeStamp = DateTime.Now;
// send notification to subscription manager
_subMgrPort.Post(new submgr.Submit(_state, DsspActions.UpdateRequest));
enableDrive.ResponsePort.Post(new DefaultUpdateResponseType());
yield break;
}
public IEnumerator<ITask> ReliableSubscribeHandler(drive.ReliableSubscribe subscribe)
{
yield return Arbiter.Choice(
SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort),
delegate(SuccessResult success)
{
_subMgrPort.Post(new submgr.Submit(
subscribe.Body.Subscriber, DsspActions.UpdateRequest, _state, null));
},
delegate(Exception ex)
{
LogError(ex);
throw ex;
}
);
}
public IEnumerator<ITask> UpdateHandler(drive.Update update)
{
_state = update.Body;
_state.TimeStamp = DateTime.Now;
update.ResponsePort.Post(new DefaultUpdateResponseType());
yield break;
}
