public IEnumerator <ITask> ResetEncodersHandler(ResetEncoders resetEncoders)
{
encoder.Reset rightResetRequest = null;
encoder.Reset leftResetRequest = null;
if (this._rightEncoderPort != null)
{
rightResetRequest = new encoder.Reset();
this._rightEncoderPort.Post(rightResetRequest);
}
if (this._leftEncoderPort != null)
{
leftResetRequest = new encoder.Reset();
this._leftEncoderPort.Post(leftResetRequest);
}
if (rightResetRequest != null)
{
yield return(rightResetRequest.ResponsePort.Choice(EmptyHandler, EmptyHandler));
}
if (leftResetRequest != null)
{
yield return(leftResetRequest.ResponsePort.Choice(EmptyHandler, EmptyHandler));
}
this.SendNotification <drive.ResetEncoders>(this._subMgrPort, resetEncoders.Body);
resetEncoders.ResponsePort.Post(new DefaultUpdateResponseType());
yield break;
}
public void DriveResetEncodersHandler(drive.ResetEncoders resetEncoders)
{
this.controllerDrivePort.Post(
new drive.ResetEncoders()
{
Body = resetEncoders.Body, ResponsePort = resetEncoders.ResponsePort
});
}
/// <summary>
/// Performs a reset on both encoders in response to
/// a reset (update) on either encoder.
/// </summary>
/// <param name="reset">An encoder Reset operation type</param>
/// <returns>An IEnumerator of type ITask</returns>
private IEnumerator <ITask> InternalEncoderReset(drive.ResetEncoders reset)
{
serialcomservice.SendAndGetRequest sg = new serialcomservice.SendAndGetRequest();
sg.Timeout = this.state.DefaultResponsePause;
sg.Terminator = board.PacketTerminatorString;
sg.Data = new serialcomservice.Packet(board.CreatePacket <byte>(board.ResetEncoderTicksString));
PortSet <serialcomservice.Packet, soap.Fault> resultPort = this.serialCOMServicePort.SendAndGet(sg);
yield return(resultPort.Choice());
soap.Fault f = (soap.Fault)resultPort;
if (f != null)
{
reset.ResponsePort.Post(f);
yield break;
}
serialcomservice.Packet p = (serialcomservice.Packet)resultPort;
if (this.HasFWError(p))
{
f = soap.Fault.FromCodeSubcodeReason(
soap.FaultCodes.Receiver,
DsspFaultCodes.OperationFailed,
"FW ERROR: Failed to reset encoder tick count!");
reset.ResponsePort.Post(f);
yield break;
}
// update both the encoder partner state as well as the brick state
this.state.DriveState.LeftWheel.EncoderState.TicksSinceReset =
this.state.DriveState.RightWheel.EncoderState.TicksSinceReset = 0;
this.state.DriveState.LeftWheel.EncoderState.CurrentAngle =
this.state.DriveState.RightWheel.EncoderState.CurrentAngle = 0;
this.state.DriveState.LeftWheel.EncoderState.CurrentReading =
this.state.DriveState.RightWheel.EncoderState.CurrentReading = 0;
this.state.DriveState.LeftWheel.EncoderState.TimeStamp =
this.state.DriveState.RightWheel.EncoderState.TimeStamp = DateTime.Now;
// Notify any brick subscribers as well as any drive subscribers
this.SendNotification <Replace>(this.submgrPort, new Replace());
this.SendNotification <drive.ResetEncoders>(this.submgrDrivePort, new drive.ResetEncoders());
reset.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
public void EncoderResetHandler(drive.ResetEncoders reset)
{
SpawnIterator <drive.ResetEncoders>(reset, this.InternalEncoderReset);
}
public void ResetEncodersHandler(diffdrive.ResetEncoders resetEncoders)
{
this.lastResetLeftWheelEncoderValue += this.state.DriveState.LeftWheel.EncoderState.CurrentReading;
this.lastResetRightWheelEncoderValue += this.state.DriveState.RightWheel.EncoderState.CurrentReading;
resetEncoders.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
