public IEnumerator <ITask> DriveAllStopHandler(drive.AllStop allstop)
{
this.state.DriveState.TimeStamp = DateTime.Now;
serialcomservice.SendAndGetRequest sg = new serialcomservice.SendAndGetRequest();
sg.Timeout = this.state.DefaultResponsePause;
sg.Data = new serialcomservice.Packet(board.CreatePacket <sbyte>(board.SetFullStopString));
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)));
allstop.ResponsePort.Post(f);
yield break;
}
if (this.HasFWError((serialcomservice.Packet)resultPort))
{
f = soap.Fault.FromCodeSubcodeReason(soap.FaultCodes.Receiver, DsspFaultCodes.OperationFailed, "Error received from FW!");
allstop.ResponsePort.Post(f);
yield break;
}
// Reflect into our state
this.state.DriveState.LeftWheel.MotorState.CurrentPower = GDDriveFullStop;
this.state.DriveState.RightWheel.MotorState.CurrentPower = GDDriveFullStop;
this.state.DriveState.LeftWheel.WheelSpeed = 0;
this.state.DriveState.RightWheel.WheelSpeed = 0;
this.state.DriveState.IsEnabled = false;
allstop.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
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);
}
/// <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 IEnumerator <ITask> DriveSetDriveSpeedHandler(drive.SetDriveSpeed setdrivespeed)
{
if (!this.state.DriveState.IsEnabled)
{
setdrivespeed.ResponsePort.Post(soap.Fault.FromCodeSubcodeReason(
soap.FaultCodes.Receiver,
DsspFaultCodes.OperationFailed,
"Drive not enabled!"));
yield break;
}
this.state.DriveState.TimeStamp = DateTime.Now;
short leftWheelSpeed = (short)(this.encoderTicksPerMeter[(int)Sides.Left] * setdrivespeed.Body.LeftWheelSpeed);
short rightWheelSpeed = (short)(this.encoderTicksPerMeter[(int)Sides.Right] * setdrivespeed.Body.RightWheelSpeed);
byte[] cmdpacket = board.CreatePacket <short>(board.SetTravelVelocityString, leftWheelSpeed, rightWheelSpeed);
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)));
setdrivespeed.ResponsePort.Post(f);
yield break;
}
if (this.HasFWError((serialcomservice.Packet)resultPort))
{
f = soap.Fault.FromCodeSubcodeReason(soap.FaultCodes.Receiver, DsspFaultCodes.OperationFailed, "Error received from FW!");
setdrivespeed.ResponsePort.Post(f);
yield break;
}
this.state.DriveState.LeftWheel.WheelSpeed = setdrivespeed.Body.LeftWheelSpeed;
this.state.DriveState.RightWheel.WheelSpeed = setdrivespeed.Body.RightWheelSpeed;
setdrivespeed.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
public IEnumerator <ITask> DriveEnableDriveHandler(drive.EnableDrive enabledrive)
{
this.state.DriveState.TimeStamp = DateTime.Now;
this.state.DriveState.IsEnabled = enabledrive.Body.Enable;
// The caller's intent is to disable the drive, so we need to set motor power to zero
if (!this.state.DriveState.IsEnabled)
{
serialcomservice.SendAndGetRequest sg = new serialcomservice.SendAndGetRequest();
sg.Timeout = this.state.DefaultResponsePause;
sg.Data = new serialcomservice.Packet(board.CreatePacket <byte>(board.SetFullStopString));
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}", board.SetFullStopString));
enabledrive.ResponsePort.Post(f);
yield break;
}
if (this.HasFWError((serialcomservice.Packet)resultPort))
{
f = soap.Fault.FromCodeSubcodeReason(soap.FaultCodes.Receiver, DsspFaultCodes.OperationFailed, "Error received from FW!");
enabledrive.ResponsePort.Post(f);
yield break;
}
// Reflect into state
this.state.DriveState.LeftWheel.MotorState.CurrentPower = GDDriveFullStop;
this.state.DriveState.RightWheel.MotorState.CurrentPower = GDDriveFullStop;
this.state.DriveState.LeftWheel.WheelSpeed = 0;
this.state.DriveState.RightWheel.WheelSpeed = 0;
}
enabledrive.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
public IEnumerator <ITask> GpioSetPinHandler(gpio.SetPin setPin)
{
soap.Fault f;
// Pins are labeled on the control board using 1-based indexing
int pinIndex = setPin.Body.PinState.Number - 1;
if (pinIndex < 0 || pinIndex >= board.GPIOPinCount)
{
f = soap.Fault.FromCodeSubcodeReason(
soap.FaultCodes.Receiver,
DsspFaultCodes.OperationFailed,
"Invalid GPIO pin index specified!");
setPin.ResponsePort.Post(f);
yield break;
}
int pinMask = 1 << pinIndex;
string cmdString =
(setPin.Body.PinState.PinDirection == gpio.GpioPinState.GpioPinDirection.Out) ?
board.SetGPIODirectionOutString : board.SetGPIODirectionInString;
byte[] cmdpacket = board.CreatePacket(cmdString, pinMask);
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());
f = (soap.Fault)resultPort;
if (f != null)
{
LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(sg.Data.Message)));
setPin.ResponsePort.Post(f);
yield break;
}
if (this.HasFWError((serialcomservice.Packet)resultPort))
{
f = soap.Fault.FromCodeSubcodeReason(
soap.FaultCodes.Receiver,
DsspFaultCodes.OperationFailed,
"Error received from FW!");
setPin.ResponsePort.Post(f);
yield break;
}
// If we have an OUT pin, then it's appropriate to take action on the HIGH/LOW state
// Ignore PinStates that have a HIGH/LOW specifier when direction is set to IN
if (setPin.Body.PinState.PinDirection == gpio.GpioPinState.GpioPinDirection.Out)
{
cmdString =
(setPin.Body.PinState.PinState == gpio.GpioPinState.GpioPinSignal.Low) ?
board.SetGPIOStateLowString : board.SetGPIOStateHighString;
cmdpacket = board.CreatePacket(cmdString, pinMask);
sg.Timeout = this.state.DefaultResponsePause;
sg.Data = new serialcomservice.Packet(cmdpacket);
resultPort = this.serialCOMServicePort.SendAndGet(sg);
yield return(resultPort.Choice());
f = (soap.Fault)resultPort;
if (f != null)
{
LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(sg.Data.Message)));
setPin.ResponsePort.Post(f);
yield break;
}
if (this.HasFWError((serialcomservice.Packet)resultPort))
{
f = soap.Fault.FromCodeSubcodeReason(
soap.FaultCodes.Receiver,
DsspFaultCodes.OperationFailed,
"Error received from FW!");
setPin.ResponsePort.Post(f);
yield break;
}
}
this.state.GpioState.Pins[pinIndex] = setPin.Body.PinState;
setPin.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
/// <summary>
/// Retrieve the COM port specified in the SerialCOMService state.
/// Perform initialization if COM port is correct and available.
/// </summary>
/// <returns>Enumerator of type ITask</returns>
private IEnumerator <ITask> InternalInitialize()
{
// Make sure we have a valid and open COM port
int currentCOMPort = 0;
yield return(this.serialCOMServicePort.GetConfiguration().Choice(
s => currentCOMPort = s.PortNumber,
f => LogError("Failed to retrieve config from Serial Port partner service")));
yield return(this.serialCOMServicePort.OpenPort().Choice(
s => LogInfo(string.Format("Opened COM{0}", currentCOMPort)),
f => LogError(string.Format("Failed to open COM{0}", currentCOMPort))));
if (currentCOMPort == 0)
{
LogError("Parallax2011ReferencePlatformIoController Service failed to initialize: Check 'PortNumber' in serialcomservice.config.xml");
this.Shutdown();
yield break;
}
this.initialized = this.Initialize();
if (this.initialized)
{
// Make sure we have a live Parallax board on the COM port by retrieving the FW version string
serialcomservice.SendAndGetRequest sg = new serialcomservice.SendAndGetRequest();
sg.Timeout = this.state.DefaultResponsePause;
sg.Terminator = board.PacketTerminatorString;
sg.Data = new serialcomservice.Packet(board.CreatePacket <byte>(board.GetVersionString));
PortSet <serialcomservice.Packet, soap.Fault> resultPort = this.serialCOMServicePort.SendAndGet(sg);
yield return(resultPort.Choice());
soap.Fault f = (soap.Fault)resultPort;
if (f == null)
{
serialcomservice.Packet p = (serialcomservice.Packet)resultPort;
if (p != null)
{
if (p.Message != null)
{
string str = Encoding.ASCII.GetString(p.Message);
this.state.FWVersion = Convert.ToInt32(str, 16);
}
}
}
else
{
LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(sg.Data.Message)));
LogError("Failed to receive FW version!");
}
LogInfo(string.Format("FW Version: {0}", this.state.FWVersion));
serialcomservice.SendAndGetRequest accSendAndGet = new serialcomservice.SendAndGetRequest();
accSendAndGet.Timeout = this.state.DefaultResponsePause;
accSendAndGet.Terminator = board.PacketTerminatorString;
accSendAndGet.Data = new serialcomservice.Packet(board.CreatePacket <ushort>(board.SetRampingValueString, (ushort)this.state.AccelerationRate));
resultPort = this.serialCOMServicePort.SendAndGet(accSendAndGet);
yield return(resultPort.Choice());
f = (soap.Fault)resultPort;
if (f != null)
{
LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(accSendAndGet.Data.Message)));
LogError("Failed to set acceleration!");
}
this.encoderTicksPerMeter = new double[]
{
this.state.DriveState.LeftWheel.EncoderState.TicksPerRevolution / (2 * this.state.DriveState.LeftWheel.Radius * Math.PI),
this.state.DriveState.RightWheel.EncoderState.TicksPerRevolution / (2 * this.state.DriveState.RightWheel.Radius * Math.PI)
};
SaveState(this.state);
base.Start();
// Make sure the pin polling port is in the main interleave because it modifies service state
MainPortInterleave.CombineWith(
new Interleave(
new ExclusiveReceiverGroup(
Arbiter.ReceiveWithIterator(true, this.pinPollingPort, this.PollPins)),
new ConcurrentReceiverGroup()));
// Start the pin polling interval
this.pinPollingPort.Post(DateTime.Now);
}
}
/// <summary>
/// Keep the FW watchdog alive by continuously retrieving pin and encoder values
/// </summary>
/// <param name="dt">A instance of type DateTime</param>
/// <returns>A instance of IEnumerator of type ITask</returns>
private IEnumerator <ITask> PollPins(DateTime dt)
{
try
{
serialcomservice.SendAndGetRequest sg = new serialcomservice.SendAndGetRequest();
sg.Timeout = this.state.DefaultResponsePause;
sg.Terminator = board.PacketTerminatorString;
sg.Data = new serialcomservice.Packet(board.CreatePacket <byte>(board.GetADCValuesString));
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)));
yield break;
}
serialcomservice.Packet p = (serialcomservice.Packet)resultPort;
if (this.HasFWError(p))
{
LogError("Error returned reading ADC values from FW!");
yield break;
}
// Stash the analog pin values into a string
string analogValues = Encoding.ASCII.GetString(p.Message);
// Retrieve the digital pin values
sg.Data = new serialcomservice.Packet(board.CreatePacket <byte>(board.GetDigitalValuesString));
resultPort = this.serialCOMServicePort.SendAndGet(sg);
yield return(resultPort.Choice());
f = (soap.Fault)resultPort;
if (f != null)
{
LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(sg.Data.Message)));
yield break;
}
p = (serialcomservice.Packet)resultPort;
if (this.HasFWError(p))
{
LogError("Error returned reading digital pin values from FW!");
yield break;
}
// Stash the digital pin values into a string
string digitalValues = Encoding.ASCII.GetString(p.Message);
// Perform all the parsing
this.UpdatePinStatesFromFWString(analogValues, digitalValues);
// Reset speed to 0 since it is currently unknown as opposed to having the last value be stuck
this.state.DriveState.LeftWheel.WheelSpeed = 0;
this.state.DriveState.RightWheel.WheelSpeed = 0;
// Retrieve both wheel speeds
sg.Data = new serialcomservice.Packet(board.CreatePacket <byte>(board.GetCurrentSpeedString));
resultPort = this.serialCOMServicePort.SendAndGet(sg);
yield return(resultPort.Choice());
p = (serialcomservice.Packet)resultPort;
if (this.HasFWError(p))
{
LogError("Error received reading speed values from FW!");
yield break;
}
f = (soap.Fault)resultPort;
if (f != null)
{
LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(sg.Data.Message)));
yield break;
}
this.UpdateSpeedStateFromFWString(Encoding.ASCII.GetString(p.Message));
// Retrieve both wheel encoder values
sg.Data = new serialcomservice.Packet(board.CreatePacket <byte>(board.GetEncoderTicksString));
resultPort = this.serialCOMServicePort.SendAndGet(sg);
yield return(resultPort.Choice());
f = (soap.Fault)resultPort;
if (f != null)
{
LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(sg.Data.Message)));
yield break;
}
p = (serialcomservice.Packet)resultPort;
if (this.HasFWError(p))
{
LogError("Error received reading encoder values from FW!");
yield break;
}
// Reflect the returned bytes into encoder and motor states
this.UpdateEncoderStatesFromFWString(Encoding.ASCII.GetString(p.Message));
}
finally
{
// Ensure we haven't been droppped
if (ServicePhase == ServiceRuntimePhase.Started)
{
// Issue another polling request
Activate(TimeoutPort(this.state.PinPollingInterval).Receive(this.pinPollingPort.Post));
}
}
}
