void Ahrs_ValueChanged(IHardwareComponent sender)
{
IAnalogSensor a = (IAnalogSensor)sender;
int val = a.AnalogValue; // 0v = 0, 5V = 470 approx
/*
* Yaw PWM val
* --------------------
*
*/
double heading;
if (val > 507)
{
heading = GeneralMath.map(val, 508, 1024, 0, 180);
}
else
{
heading = GeneralMath.map(val, 0, 507, 180, 360);
}
//Debug.WriteLine("Ahrs: Value=" + val + " heading: " + heading);
lock (currentSensorsDataLock)
{
ISensorsData sensorsData = new SensorsDataShorty(this.currentSensorsData);
sensorsData.CompassHeadingDegrees = heading;
//Debug.WriteLine(sensorsData.ToString());
this.currentSensorsData = sensorsData;
}
}
public ComponentController(CraftMyPcContext context, IHardwareUnit hardwareUnit,
IHardwareComponent hardwareComponent,
ICategoryFilter categoryFilter)
{
_context = context;
_hardwareUnit = hardwareUnit;
_hardwareComponent = hardwareComponent;
_categoryFilter = categoryFilter;
}
void psonar_DistanceChanged(IHardwareComponent sender)
{
RangeFRmeters = parkingSonar.RangeFRcm * 0.01d;
RangeFLmeters = parkingSonar.RangeFLcm * 0.01d;
RangeBRmeters = parkingSonar.RangeBRcm * 0.01d;
RangeBLmeters = parkingSonar.RangeBLcm * 0.01d;
Timestamp = DateTime.Now;
OnDistanceChanged(new double[] { RangeFRmeters, RangeFLmeters, RangeBRmeters, RangeBLmeters });
}
void psonar_DistanceChanged(IHardwareComponent sender)
{
RangeFRmeters = parkingSonar.RangeFRcm * 0.01d;
RangeFLmeters = parkingSonar.RangeFLcm * 0.01d;
RangeBRmeters = parkingSonar.RangeBRcm * 0.01d;
RangeBLmeters = parkingSonar.RangeBLcm * 0.01d;
Timestamp = DateTime.Now;
OnDistanceChanged(new double[] { RangeFRmeters, RangeFLmeters, RangeBRmeters, RangeBLmeters });
}
private void SystemComponentWearingOff(IHardwareComponent component)
{
if (component.IsAlive)
{
bool componentDown = RandomEvent.Instance.Occured(1 - GetProbability(component.FailureRate));
if (componentDown)
{
component.IsAlive = false;
}
}
}
void ir_DistanceChanged(IHardwareComponent sender)
{
//Debug.WriteLine("sender: " + sender + " Value=" + gp2d12.Value.ToString());
//Debug.WriteLine("IR: " + gp2d12.Value.ToString() + String.Format(" = {0:0.00} units", gp2d12.Distance));
double rangeMeters = gp2d12.Distance * metersPerUnitIR_10_80;
if (rangeMeters != RangeMeters)
{
RangeMeters = rangeMeters;
Timestamp = DateTime.Now;
OnDistanceChanged();
}
}
void ir_DistanceChanged(IHardwareComponent sender)
{
//Debug.WriteLine("sender: " + sender + " Value=" + gp2d12.Value.ToString());
//Debug.WriteLine("IR: " + gp2d12.Value.ToString() + String.Format(" = {0:0.00} units", gp2d12.Distance));
double rangeMeters = gp2d12.Distance * metersPerUnitIR_20_150;
if (rangeMeters != RangeMeters)
{
RangeMeters = rangeMeters;
Timestamp = DateTime.Now;
OnDistanceChanged();
}
}
// registration methods
public void RegisterHwComponent(IHardwareComponent hwComp, int registrationID, string name)
{
if (hwComp == null)
{
throw new Exception("The hardware component must be valid!");
}
if (hwComponentMap.Contains(registrationID))
{
throw new Exception("The hardware component is already registered!");
}
hwComp.Name = name;
hwComponentMap.Add(registrationID, hwComp);
}
void ir_DistanceChanged(IHardwareComponent sender)
{
//Debug.WriteLine("sender: " + sender + " Value=" + srf04.Distance.ToString());
//Debug.WriteLine("Sonar: " + srf04.Distance.ToString() + String.Format(" = {0:0.00} units", srf04.Distance));
double rangeMeters = srf04.Distance * metersPerUnitSonar;
// we cannot trust minimum readings, the SRF04 sonar often goes to near 0 (0-9cm) for unknown reasons.
if (rangeMeters > MinDistanceMeters && rangeMeters != RangeMeters)
{
RangeMeters = rangeMeters;
Timestamp = DateTime.Now;
OnDistanceChanged();
}
}
void ir_DistanceChanged(IHardwareComponent sender)
{
//Debug.WriteLine("sender: " + sender + " Value=" + srf04.Distance.ToString());
//Debug.WriteLine("Sonar: " + srf04.Distance.ToString() + String.Format(" = {0:0.00} units", srf04.Distance));
double rangeMeters = srf04.Distance * metersPerUnitSonar;
// we cannot trust minimum readings, the SRF04 sonar often goes to near 0 (0-9cm) for unknown reasons.
if (rangeMeters > MinDistanceMeters && rangeMeters != RangeMeters)
{
RangeMeters = rangeMeters;
Timestamp = DateTime.Now;
OnDistanceChanged();
}
}
private void Ahrs_ValuesChanged(IHardwareComponent sender)
{
// AHRS is based on Arduino Motion Plug sketch, a pro-mini is connected to Plucky Wheels using I2C.
IAhrs ahrs = (IAhrs)sender;
double heading = Direction.to360(ahrs.Yaw);
//Debug.WriteLine("Compass: heading=" + heading);
lock (currentSensorsDataLock)
{
ISensorsData sensorsData = new SensorsDataPlucky(this.currentSensorsData);
sensorsData.CompassHeadingDegrees = heading;
//Debug.WriteLine(sensorsData.ToString());
this.currentSensorsData = sensorsData;
}
}
void Compass_HeadingChanged(IHardwareComponent sender)
{
// The cmps03 command queries a Devantech CMPS03 Electronic compass module attached to the Serializer’s I2C port.
// The current heading is returned in Binary Radians, or BRADS. To convert BRADS to DEGREES, multiply BRADS by 360/255 (~1.41).
ICompassCMPS03 cmps = (ICompassCMPS03)sender;
double heading = Math.Round(cmps.Heading * 360.0d / 255.0d, 1);
Debug.WriteLine("Compass: heading=" + heading);
lock (currentSensorsDataLock)
{
ISensorsData sensorsData = new SensorsDataShorty(this.currentSensorsData);
sensorsData.CompassHeadingDegrees = heading;
//Debug.WriteLine(sensorsData.ToString());
this.currentSensorsData = sensorsData;
}
}
void batteryVoltage_ValueChanged(IHardwareComponent sender)
{
lock (currentSensorsDataLock)
{
SensorsDataShorty sensorsData = new SensorsDataShorty(this.currentSensorsData);
IAnalogSensor bv = sender as IAnalogSensor;
Debug.Assert(bv != null, "SensorsControllerElement: batteryVoltage_ValueChanged(): AnalogSensor must be non-null");
// analog pin 5 in Element board is internally tied to 1/3 of the supply voltage level. The 5.0d is 5V, microcontroller's ADC reference and 1024 is range.
double voltage = 3.0d * (bv.AnalogValue * 5.0d) / 1024.0d;
sensorsData.BatteryVoltage = voltage; // also sets sensorsData.BatteryVoltageTimestamp
this.currentSensorsData = sensorsData;
}
Debug.WriteLine(this.currentSensorsData.ToString());
}
private void encoder_CountChanged(IHardwareComponent sender)
{
lock (currentSensorsDataLock)
{
SensorsDataPlucky sensorsData = new SensorsDataPlucky(this.currentSensorsData);
IWheelEncoder encoder = sender as IWheelEncoder;
Debug.Assert(encoder != null, "SensorsControllerPlucky: encoder_CountChanged(): Encoder must be non-null");
if (encoder.WheelEncoderId == WheelEncoderId.Encoder2)
{
sensorsData.WheelEncoderLeftTicks = encoder.Count;
}
else if (encoder.WheelEncoderId == WheelEncoderId.Encoder1)
{
sensorsData.WheelEncoderRightTicks = encoder.Count;
}
//Debug.WriteLine(sensorsData.ToString());
this.currentSensorsData = sensorsData;
}
}
private void batteryVoltage_ValueChanged(IHardwareComponent sender)
{
lock (currentSensorsDataLock)
{
SensorsDataPlucky sensorsData = new SensorsDataPlucky(this.currentSensorsData);
IAnalogSensor bv = sender as IAnalogSensor;
Debug.Assert(bv != null, "SensorsControllerPlucky: batteryVoltage_ValueChanged(): AnalogSensor must be non-null");
// analog pin 5 in Element board is internally tied to 1/3 of the supply voltage level.
// The 5.0d is 5V, microcontroller's ADC reference and 1024 is range.
// on Plucky the battery voltage goes through 1/3 divider and comes to Arduino Pin 3, which is reported as Pin 5 to here.
// see PluckyWheels.ino sketch
double voltage = 3.0d * (bv.AnalogValue * 5.0d) / 1024.0d;
sensorsData.BatteryVoltage = voltage; // also sets sensorsData.BatteryVoltageTimestamp
this.currentSensorsData = sensorsData;
}
Debug.WriteLine(this.currentSensorsData.ToString());
}
void Ahrs_ValueChanged(IHardwareComponent sender)
{
IAnalogSensor a = (IAnalogSensor)sender;
int val = a.AnalogValue; // 0v = 0, 5V = 470 approx
/*
Yaw PWM val
--------------------
*/
double heading;
if (val > 507)
{
heading = GeneralMath.map(val, 508, 1024, 0, 180);
}
else
{
heading = GeneralMath.map(val, 0, 507, 180, 360);
}
//Debug.WriteLine("Ahrs: Value=" + val + " heading: " + heading);
lock (currentSensorsDataLock)
{
SensorsData sensorsData = new SensorsData(this.currentSensorsData);
sensorsData.CompassHeadingDegrees = heading;
//Debug.WriteLine(sensorsData.ToString());
this.currentSensorsData = sensorsData;
}
}
private void batteryVoltage_ValueChanged(IHardwareComponent sender)
{
lock (currentSensorsDataLock)
{
SensorsData sensorsData = new SensorsData(this.currentSensorsData);
IAnalogSensor bv = sender as IAnalogSensor;
Debug.Assert(bv != null, "SensorsControllerPlucky: batteryVoltage_ValueChanged(): AnalogSensor must be non-null");
// analog pin 5 in Element board is internally tied to 1/3 of the supply voltage level.
// The 5.0d is 5V, microcontroller's ADC reference and 1024 is range.
// on Plucky the battery voltage goes through 1/3 divider and comes to Arduino Pin 3, which is reported as Pin 5 to here.
// see PluckyWheels.ino sketch
double voltage = 3.0d * (bv.AnalogValue * 5.0d) / 1024.0d;
sensorsData.BatteryVoltage = voltage; // also sets sensorsData.BatteryVoltageTimestamp
this.currentSensorsData = sensorsData;
}
Debug.WriteLine(this.currentSensorsData.ToString());
}
private void element_CommunicationStopped(IHardwareComponent sender)
{
communicating = false;
OnStopCommunication();
}
private void element_CommunicationStarted(IHardwareComponent sender)
{
communicating = true;
OnStartCommunication();
}
private void element_CommunicationStopped(IHardwareComponent sender)
{
communicating = false;
OnStopCommunication();
}
void encoder_CountChanged(IHardwareComponent sender)
{
lock (currentSensorsDataLock)
{
SensorsData sensorsData = new SensorsData(this.currentSensorsData);
IWheelEncoder encoder = sender as IWheelEncoder;
Debug.Assert(encoder != null, "SensorsControllerElement: encoder_CountChanged(): Encoder must be non-null");
if (encoder.WheelEncoderId == WheelEncoderId.Encoder2)
{
sensorsData.WheelEncoderLeftTicks = encoder.Count;
}
else if (encoder.WheelEncoderId == WheelEncoderId.Encoder1)
{
sensorsData.WheelEncoderRightTicks = encoder.Count;
}
//Debug.WriteLine(sensorsData.ToString());
this.currentSensorsData = sensorsData;
}
}
void batteryVoltage_ValueChanged(IHardwareComponent sender)
{
lock (currentSensorsDataLock)
{
SensorsData sensorsData = new SensorsData(this.currentSensorsData);
IAnalogSensor bv = sender as IAnalogSensor;
Debug.Assert(bv != null, "SensorsControllerElement: batteryVoltage_ValueChanged(): AnalogSensor must be non-null");
// analog pin 5 in Element board is internally tied to 1/3 of the supply voltage level. The 5.0d is 5V, microcontroller's ADC reference and 1024 is range.
double voltage = 3.0d * (bv.AnalogValue * 5.0d) / 1024.0d;
sensorsData.BatteryVoltage = voltage; // also sets sensorsData.BatteryVoltageTimestamp
this.currentSensorsData = sensorsData;
}
Debug.WriteLine(this.currentSensorsData.ToString());
}
void Compass_HeadingChanged(IHardwareComponent sender)
{
// The cmps03 command queries a Devantech CMPS03 Electronic compass module attached to the Serializer’s I2C port.
// The current heading is returned in Binary Radians, or BRADS. To convert BRADS to DEGREES, multiply BRADS by 360/255 (~1.41).
ICompassCMPS03 cmps = (ICompassCMPS03)sender;
double heading = Math.Round(cmps.Heading * 360.0d / 255.0d, 1);
Debug.WriteLine("Compass: heading=" + heading);
lock (currentSensorsDataLock)
{
SensorsData sensorsData = new SensorsData(this.currentSensorsData);
sensorsData.CompassHeadingDegrees = heading;
//Debug.WriteLine(sensorsData.ToString());
this.currentSensorsData = sensorsData;
}
}
private void element_CommunicationStarted(IHardwareComponent sender)
{
communicating = true;
OnStartCommunication();
}
private void bridge_CommunicationStarted(IHardwareComponent sender)
{
Debug.WriteLine("RobotShortyHardwareBridge: bridge_CommunicationStarted: Communication started");
//isBrickComStarted = true;
}
private void bridge_CommunicationStarted(IHardwareComponent sender)
{
Debug.WriteLine("RobotShortyHardwareBridge: bridge_CommunicationStarted: Communication started");
//isBrickComStarted = true;
}
private void Ahrs_ValuesChanged(IHardwareComponent sender)
{
// AHRS is based on Arduino Motion Plug sketch, a pro-mini is connected to Plucky Wheels using I2C.
IAhrs ahrs = (IAhrs)sender;
double heading = Direction.to360(ahrs.Yaw);
//Debug.WriteLine("Compass: heading=" + heading);
lock (currentSensorsDataLock)
{
SensorsData sensorsData = new SensorsData(this.currentSensorsData);
sensorsData.CompassHeadingDegrees = heading;
//Debug.WriteLine(sensorsData.ToString());
this.currentSensorsData = sensorsData;
}
}
