/// <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 void DriveDriveDistanceHandler(drive.DriveDistance drivedistance)
{
this.controllerDrivePort.Post(
new drive.DriveDistance()
{
Body = drivedistance.Body, ResponsePort = drivedistance.ResponsePort
});
}
public IEnumerator <ITask> DriveDriveDistanceHandler(drive.DriveDistance drivedistance)
{
if (!this.state.DriveState.IsEnabled)
{
drivedistance.ResponsePort.Post(soap.Fault.FromCodeSubcodeReason(
soap.FaultCodes.Receiver,
DsspFaultCodes.OperationFailed,
"Drive not enabled!"));
yield break;
}
this.state.DriveState.TimeStamp = DateTime.Now;
short pwr = (short)(board.HBridgeReverseMax +
((drivedistance.Body.Power - GDDriveMin)
* DrivePowerScale));
short dist = (short)(this.encoderTicksPerMeter[(int)Sides.Left] * drivedistance.Body.Distance);
byte[] cmdpacket = board.CreatePacket <short, short>(board.SetTravelDistanceString, dist, pwr);
serialcomservice.SendAndGetRequest sg = new serialcomservice.SendAndGetRequest();
sg.Timeout = this.state.DefaultResponsePause;
sg.Data = new serialcomservice.Packet(cmdpacket);
var resultPort = this.serialCOMServicePort.SendAndGet(sg);
yield return(resultPort.Choice());
soap.Fault f = (soap.Fault)resultPort;
if (f != null)
{
LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(sg.Data.Message)));
drivedistance.ResponsePort.Post(f);
yield break;
}
if (this.HasFWError((serialcomservice.Packet)resultPort))
{
f = soap.Fault.FromCodeSubcodeReason(
soap.FaultCodes.Receiver,
DsspFaultCodes.OperationFailed,
"Error received from FW!");
drivedistance.ResponsePort.Post(f);
yield break;
}
this.state.DriveState.LeftWheel.MotorState.CurrentPower = drivedistance.Body.Power;
this.state.DriveState.RightWheel.MotorState.CurrentPower = drivedistance.Body.Power;
this.state.DriveState.DriveDistanceStage = drive.DriveStage.Completed;
drivedistance.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
public IEnumerator <ITask> DriveDistanceHandler(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>
/// drives a specified number of meters
/// </summary>
IEnumerator <ITask> DriveUntilDistance(double distance, double power)
{
//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
int stopLeftWheelAt = (int)Math.Round(Math.Abs(distance) / (2.0 * 3.14159 * _state.LeftWheel.Radius / _state.LeftWheel.EncoderState.TicksPerRevolution));
int stopRightWheelAt = (int)Math.Round(Math.Abs(distance) / (2.0 * 3.14159 * _state.RightWheel.Radius / _state.RightWheel.EncoderState.TicksPerRevolution));
// Subscribe to the encoders on internal ports
encoder.EncoderOperations leftNotificationPort = new encoder.EncoderOperations();
encoder.EncoderOperations rightNotificationPort = new encoder.EncoderOperations();
encoder.Subscribe op = new encoder.Subscribe();
op.Body = new SubscribeRequestType();
op.NotificationPort = leftNotificationPort;
_leftEncoderPort.Post(op);
yield return(Arbiter.Choice(op.ResponsePort,
delegate(SubscribeResponseType response)
{
//subscription was successful, start listening for encoder replace messages
Activate(Arbiter.Receive <encoder.UpdateTickCount>(false, leftNotificationPort,
delegate(encoder.UpdateTickCount update)
{
StopMotorWithEncoderHandler(leftNotificationPort, update, _leftMotorPort, stopLeftWheelAt);
}));
},
delegate(Fault fault) { LogError(fault); }
));
encoder.Subscribe op2 = new encoder.Subscribe();
op2.Body = new SubscribeRequestType();
op2.NotificationPort = rightNotificationPort;
_leftEncoderPort.Post(op2);
yield return(Arbiter.Choice(op2.ResponsePort,
delegate(SubscribeResponseType response)
{
//subscription was successful, start listening for encoder replace messages
Activate(Arbiter.Receive <encoder.UpdateTickCount>(false, rightNotificationPort,
delegate(encoder.UpdateTickCount update)
{
StopMotorWithEncoderHandler(rightNotificationPort, update, _rightMotorPort, stopRightWheelAt);
}
));
},
delegate(Fault fault) { LogError(fault); }
));
//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 = DriveStage.Started;
DriveDistance driveUpdate = new DriveDistance();
driveUpdate.Body.DriveDistanceStage = DriveStage.Started;
_internalDriveOperationsPort.Post(driveUpdate);
Motor.SetMotorPower leftPower = new Motor.SetMotorPower(Pow);
leftPower.ResponsePort = responsePort;
_leftMotorPort.Post(leftPower);
Motor.SetMotorPower rightPower = new Motor.SetMotorPower(Pow);
rightPower.ResponsePort = responsePort;
_rightMotorPort.Post(rightPower);
Activate(Arbiter.MultipleItemReceive <DefaultUpdateResponseType, Fault>(responsePort, 2,
delegate(ICollection <DefaultUpdateResponseType> successList, ICollection <Fault> failureList)
{
foreach (Fault fault in failureList)
{
LogError(fault);
}
}
));
// send notification of driveDistance complete to subscription manager
driveUpdate.Body.DriveDistanceStage = DriveStage.Completed;
_internalDriveOperationsPort.Post(driveUpdate);
_state.DriveDistanceStage = DriveStage.Completed;
}
public IEnumerator <ITask> DriveDistanceHandler(drive.DriveDistance driveDistance)
{
LogError("Drive distance is not implemented");
yield break;
}
public void DriveDistanceHandler(diffdrive.DriveDistance driveDistance)
{
if (this.driveEntity == null)
{
throw new InvalidOperationException("Simulation entity not registered with service");
}
if (!this.state.DriveState.IsEnabled)
{
driveDistance.ResponsePort.Post(Fault.FromException(new Exception("Drive is not enabled.")));
LogError("DriveDistance request to disabled drive.");
return;
}
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.");
return;
}
this.state.DriveState.DriveDistanceStage = driveDistance.Body.DriveDistanceStage;
if (driveDistance.Body.DriveDistanceStage == diffdrive.DriveStage.InitialRequest)
{
var entityResponse = new Port <engine.OperationResult>();
Activate(
Arbiter.Receive(
false,
entityResponse,
result =>
{
// post a message to ourselves indicating that the drive distance has completed
var req = new diffdrive.DriveDistanceRequest(0, 0);
switch (result)
{
case engine.OperationResult.Error:
req.DriveDistanceStage = diffdrive.DriveStage.Canceled;
break;
case engine.OperationResult.Canceled:
req.DriveDistanceStage = diffdrive.DriveStage.Canceled;
break;
case engine.OperationResult.Completed:
req.DriveDistanceStage = diffdrive.DriveStage.Completed;
break;
}
this.drivePort.Post(new diffdrive.DriveDistance(req));
}));
this.driveEntity.DriveDistance(
(float)driveDistance.Body.Distance, (float)driveDistance.Body.Power, entityResponse);
var req2 = new diffdrive.DriveDistanceRequest(0, 0)
{
DriveDistanceStage = diffdrive.DriveStage.Started
};
this.drivePort.Post(new diffdrive.DriveDistance(req2));
}
else
{
SendNotification(this.subMgrPort, driveDistance);
}
driveDistance.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
