/// <summary>
/// Keep the FW watchdog alive by continuously retrieving sensor+encoder values
/// </summary>
/// <param name="dt">A instance of type DateTime</param>
/// <returns>A instance of IEnumerator of type ITask</returns>
private IEnumerator <ITask> PollSensors(DateTime dt)
{
try
{
soap.Fault fault = null;
adc.ADCPinArrayState pinArrayState = null;
adc.Get adcGet = new adc.Get();
this.controllerAdcPinArrayPort.Post(adcGet);
yield return(adcGet.ResponsePort.Choice());
fault = (soap.Fault)adcGet.ResponsePort;
if (fault != null)
{
LogError(string.Format("Fault on getting ADCPinArray state: {0}", fault.Detail));
throw fault.ToException();
}
pinArrayState = (adc.ADCPinArrayState)adcGet.ResponsePort;
for (int i = 0; i < this.state.InfraredSensorState.Sensors.Count; i++)
{
adc.ADCPinState pinState = pinArrayState.Pins[this.state.InfraredSensorState.Sensors[i].HardwareIdentifier];
this.state.InfraredSensorState.Sensors[i].DistanceMeasurement = this.state.InfraredRawValueDivisorScalar * Math.Pow(pinState.PinValue, this.state.InfraredDistanceExponent) / CentimetersPerMeter;
}
for (int i = 0; i < this.state.SonarSensorState.Sensors.Count; i++)
{
adc.ADCPinState pinState = pinArrayState.Pins[this.state.SonarSensorState.Sensors[i].HardwareIdentifier];
this.state.SonarSensorState.Sensors[i].DistanceMeasurement = pinState.PinValue * this.state.SonarTimeValueMultiplier / CentimetersPerMeter;
}
this.state.BatteryState.PercentBatteryPower = (pinArrayState.Pins[this.state.BatteryVoltagePinIndex].PinValue * this.state.BatteryVoltageDivider) / this.state.BatteryState.MaxBatteryPower * 100.0;
if (this.state.BatteryState.PercentBatteryPower <= this.state.BatteryState.PercentCriticalBattery)
{
battery.BatteryNotification batteryNotification = new battery.BatteryNotification(
(int)this.state.BatteryState.MaxBatteryPower,
this.state.BatteryState.PercentBatteryPower);
this.SendNotification <battery.Replace>(this.submgrBatteryPort, batteryNotification);
}
}
finally
{
// Ensure we haven't been droppped
if (ServicePhase == ServiceRuntimePhase.Started)
{
// Issue another polling request
Activate(TimeoutPort(this.state.SensorPollingInterval).Receive(this.sensorPollingPort.Post));
}
}
yield break;
}
/// <summary>
/// Allocation and assignments for first run
/// </summary>
/// <returns>True if initialization succeeded, otherwise False</returns>
private bool Initialize()
{
if (this.initialized)
{
return(this.initialized);
}
try
{
// No persisted state file, create a new one
if (this.state == null)
{
this.state = new Parallax2011ReferencePlatformIoControllerState();
}
// Fill out the L/R drive state specifics
if (this.state.DriveState.LeftWheel == null || this.state.DriveState.LeftWheel.Radius == 0)
{
this.state.DriveState.LeftWheel = new motor.WheeledMotorState()
{
Radius = board.DefaultWheelRadius
};
}
this.state.DriveState.LeftWheel.MotorState = new motor.MotorState();
this.state.DriveState.LeftWheel.MotorState.Name = this.wheelNames[(int)Sides.Left];
this.state.DriveState.LeftWheel.MotorState.HardwareIdentifier = (int)Sides.Left;
this.state.DriveState.LeftWheel.MotorState.PowerScalingFactor = 1;
if (this.state.DriveState.LeftWheel.EncoderState == null || this.state.DriveState.LeftWheel.EncoderState.TicksPerRevolution == 0)
{
this.state.DriveState.LeftWheel.EncoderState = new Services.Encoder.EncoderState()
{
TicksPerRevolution = board.DefaultTicksPerRevolution
};
}
if (this.state.DriveState.RightWheel == null || this.state.DriveState.RightWheel.Radius == 0)
{
this.state.DriveState.RightWheel = new motor.WheeledMotorState()
{
Radius = board.DefaultWheelRadius
};
}
this.state.DriveState.RightWheel.MotorState = new motor.MotorState();
this.state.DriveState.RightWheel.MotorState.Name = this.wheelNames[(int)Sides.Right];
this.state.DriveState.RightWheel.MotorState.HardwareIdentifier = (int)Sides.Right;
this.state.DriveState.RightWheel.MotorState.PowerScalingFactor = 1;
if (this.state.DriveState.RightWheel.EncoderState == null || this.state.DriveState.RightWheel.EncoderState.TicksPerRevolution == 0)
{
this.state.DriveState.RightWheel.EncoderState = new Services.Encoder.EncoderState()
{
TicksPerRevolution = board.DefaultTicksPerRevolution
};
}
// Reflect all the default GPIO pin states into the state
if (this.state.GpioState == null)
{
this.state.GpioState = new gpio.GpioPinArrayState();
}
this.state.GpioState.Pins = new List <gpio.GpioPinState>(board.GPIOPinCount);
for (int pinIndex = 0; pinIndex < board.GPIOPinCount; pinIndex++)
{
gpio.GpioPinState pinState = new gpio.GpioPinState();
// GPIO bank defaults to IN for PING sensors
pinState.PinDirection = gpio.GpioPinState.GpioPinDirection.In;
// Pins are labeled on the control board using 1-based indexing
pinState.Number = pinIndex + 1;
this.state.GpioState.Pins.Add(pinState);
}
// Populate the ADCPins
// NOTE: Pins on the board are labelled using 1-based indexing
if (this.state.AdcState == null)
{
this.state.AdcState = new adc.ADCPinArrayState();
}
this.state.AdcState.Pins = new List <adc.ADCPinState>(board.ADCPinCount + board.DigitalPinCount);
for (int pinIndex = 0; pinIndex < board.ADCPinCount + board.DigitalPinCount; pinIndex++)
{
adc.ADCPinState a = new adc.ADCPinState();
a.Name = string.Format("Pin_{0}", pinIndex);
a.TimeStamp = DateTime.Now;
a.Number = pinIndex + 1;
this.state.AdcState.Pins.Add(a);
}
}
catch (Exception e)
{
LogError(e);
this.Shutdown();
return(false);
}
this.state.DriveState.TimeStamp =
this.state.LastStartTime = DateTime.Now;
SaveState(this.state);
return(true);
}
