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 RangerDistanceChangedEvent(object sender, RangerSensorEventArgs e)
{
//Debug.WriteLine("Ranger: " + e.Name + " RangeMeters=" + e.RangeMeters);
lock (currentSensorsDataLock)
{
SensorsData sensorsData = new SensorsData(this.currentSensorsData);
switch (e.Name)
{
case "IrLeft":
sensorsData.IrLeftMeters = e.RangeMeters[0];
sensorsData.IrLeftMetersTimestamp = e.TimeTicks;
break;
case "IrRight":
sensorsData.IrRightMeters = e.RangeMeters[0];
sensorsData.IrRightMetersTimestamp = e.TimeTicks;
break;
case "IrFront":
sensorsData.IrFrontMeters = e.RangeMeters[0];
sensorsData.IrFrontMetersTimestamp = e.TimeTicks;
break;
case "IrRear":
sensorsData.IrRearMeters = e.RangeMeters[0];
sensorsData.IrRearMetersTimestamp = e.TimeTicks;
break;
case "SonarLeft":
sensorsData.RangerFrontLeftMeters = e.RangeMeters[0];
sensorsData.RangerFrontLeftMetersTimestamp = e.TimeTicks;
break;
case "SonarRight":
sensorsData.RangerFrontRightMeters = e.RangeMeters[0];
sensorsData.RangerFrontRightMetersTimestamp = e.TimeTicks;
break;
}
//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 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;
}
}
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;
}
}
/// <summary>
/// we can create sensors here, but cannot send commands before bridge_CommunicationStarted is called in PluckyTheRobot
/// for example, encoder.Clear() will hang.
/// </summary>
public async Task InitSensors(CancellationTokenSource cts)
{
// see C:\Projects\Serializer_3_0\ExampleApps\AnalogSensorExample\AnalogSensorExample\Form1.cs
// Note: the Element board communicates at 19200 Baud, which is roughly 1.5 kbytes/sec
// Comm link is busy with motor commands and has to be responsive to encoders, for odometry to work.
// Sensors must carefully adjust their demands by setting UpdateFrequency and Enabled properties.
// *********************** infrared rangers:
SensorPose spIrLeft = new SensorPose() { XMeters = 0.0d, YMeters = 0.1d, ThetaRadians = Math.PI / 2.0d };
RangerIrLeft = RangerSensorFactory.produceRangerSensor(RangerSensorFactoryProducts.RangerSensorIR10_80, "IrLeft", spIrLeft,
hardwareBrick, AnalogPinId.A1, rangersSamplingIntervalMs, rangersSensitivityThresholdCm);
RangerIrLeft.distanceChangedEvent += new EventHandler<RangerSensorEventArgs>(RangerDistanceChangedEvent);
RangerSensors.Add(RangerIrLeft.Name, RangerIrLeft);
SensorPose spIrRight = new SensorPose() { XMeters = 0.0d, YMeters = -0.1d, ThetaRadians = -Math.PI / 2.0d };
RangerIrRight = RangerSensorFactory.produceRangerSensor(RangerSensorFactoryProducts.RangerSensorIR10_80, "IrRight", spIrRight,
hardwareBrick, AnalogPinId.A0, rangersSamplingIntervalMs, rangersSensitivityThresholdCm);
RangerIrRight.distanceChangedEvent += new EventHandler<RangerSensorEventArgs>(RangerDistanceChangedEvent);
RangerSensors.Add(RangerIrRight.Name, RangerIrRight);
SensorPose spIrFront = new SensorPose() { XMeters = 0.1d, YMeters = 0.0d, ThetaRadians = 0.0d };
RangerIrFront = RangerSensorFactory.produceRangerSensor(RangerSensorFactoryProducts.RangerSensorIR10_80, "IrFront", spIrFront,
hardwareBrick, AnalogPinId.A3, rangersSamplingIntervalMs, rangersSensitivityThresholdCm);
RangerIrFront.distanceChangedEvent += new EventHandler<RangerSensorEventArgs>(RangerDistanceChangedEvent);
RangerSensors.Add(RangerIrFront.Name, RangerIrFront);
SensorPose spIrRear = new SensorPose() { XMeters = -0.1d, YMeters = 0.0d, ThetaRadians = Math.PI };
RangerIrRear = RangerSensorFactory.produceRangerSensor(RangerSensorFactoryProducts.RangerSensorIR10_80, "IrRear", spIrRear,
hardwareBrick, AnalogPinId.A2, rangersSamplingIntervalMs, rangersSensitivityThresholdCm);
RangerIrRear.distanceChangedEvent += new EventHandler<RangerSensorEventArgs>(RangerDistanceChangedEvent);
RangerSensors.Add(RangerIrRear.Name, RangerIrRear);
// *********************** ultrasonic rangers:
SensorPose spSonarLeft = new SensorPose() { XMeters = 0.1d, YMeters = 0.05d, ThetaRadians = Math.PI / 6.0d };
RangerSonarLeft = RangerSensorFactory.produceRangerSensor(RangerSensorFactoryProducts.RangerSensorSonar, "SonarLeft", spSonarLeft,
hardwareBrick, GpioPinId.Pin4, GpioPinId.Pin5, rangersSamplingIntervalMs, rangersSensitivityThresholdCm);
RangerSonarLeft.distanceChangedEvent += new EventHandler<RangerSensorEventArgs>(RangerDistanceChangedEvent);
RangerSensors.Add(RangerSonarLeft.Name, RangerSonarLeft);
SensorPose spSonarRight = new SensorPose() { XMeters = 0.1d, YMeters = -0.05d, ThetaRadians = -Math.PI / 6.0d };
RangerSonarRight = RangerSensorFactory.produceRangerSensor(RangerSensorFactoryProducts.RangerSensorSonar, "SonarRight", spSonarRight,
hardwareBrick, GpioPinId.Pin2, GpioPinId.Pin3, rangersSamplingIntervalMs, rangersSensitivityThresholdCm);
RangerSonarRight.distanceChangedEvent += new EventHandler<RangerSensorEventArgs>(RangerDistanceChangedEvent);
RangerSensors.Add(RangerSonarRight.Name, RangerSonarRight);
// *********************** wheel encoders - roughly 500 ticks per wheel revolution, 1200 ticks per meter.
encoderLeft = CreateWheelEncoder(hardwareBrick, WheelEncoderId.Encoder2, (int)mainLoopCycleMs, encodersSensitivityThresholdTicks);
encoderRight = CreateWheelEncoder(hardwareBrick, WheelEncoderId.Encoder1, (int)mainLoopCycleMs, encodersSensitivityThresholdTicks);
Compass = hardwareBrick.produceCompassCMPS03(0x60, CompassSamplingIntervalMs, CompassSensitivityThreshold);
Compass.HeadingChanged += new HardwareComponentEventHandler(Compass_HeadingChanged);
// arduino based AHRS - PWM DAC to pin 4:
Ahrs = hardwareBrick.produceAnalogSensor(AnalogPinId.A4, CompassSamplingIntervalMs, CompassSensitivityThreshold);
Ahrs.AnalogValueChanged += new HardwareComponentEventHandler(Ahrs_ValueChanged);
// arduino based AHRS - I2C does not connect:
//Ahrs = new I2CDevice(hardwareBrick);
//Ahrs.I2CAddress = 173; // The Wire library uses 7 bit addresses throughout.
// // If you have a datasheet or sample code that uses 8 bit address, you'll want to drop the low bit (i.e. shift the value one bit to the right),
// // yielding an address between 0 and 127. However the addresses from 0 to 7 are not used because are reserved so the first address that can be used is 8.
//Ahrs.WriteCommand = "234";
//Ahrs.ReadCommand = "6";
await PixyCameraSensor.Open(cts);
PixyCameraSensor.TargetingCameraTargetsChanged += PixyCameraSensor_PixyCameraBlocksChanged;
batteryVoltage = CreateBatteryVoltageMeter(hardwareBrick, batterySamplingIntervalMs, batterySensitivityThresholdVolts);
batteryVoltage.Enabled = true; // slow update rate, leave it turned on
IrRangersEnabled = true; // if false, do not sample or update
SonarsEnabled = false; // will enable sonars with an interval 50ms to avoid interference
EncodersEnabled = true;
//CompassEnabled = true; // compass no good inside concrete buildings, use gyro instead
CompassEnabled = false;
Ahrs.Enabled = true;
PixyCameraSensor.Enabled = true;
currentSensorsData = new SensorsData() { RangerSensors = this.RangerSensors };
}
void PixyCameraSensor_PixyCameraBlocksChanged(object sender, TargetingCameraEventArgs args)
{
//Debug.WriteLine("Pixy Camera Event: " + args);
// On Arduino:
// pixy.blocks[i].signature The signature number of the detected object (1-7)
// pixy.blocks[i].x The x location of the center of the detected object (0 to 319)
// pixy.blocks[i].y The y location of the center of the detected object (0 to 199)
// pixy.blocks[i].width The width of the detected object (1 to 320)
// pixy.blocks[i].height The height of the detected object (1 to 200)
// Field of view:
// goal 45 degrees left x=10
// middle x=160
// goal 45 degrees right x=310
//
// goal 30 degrees up y=10
// middle y=90
// goal 30 degrees down y=190
//
if (args.width * args.height > 500) // only large objects count
{
int bearing = GeneralMath.map(args.x, 0, 320, -45, 45);
int inclination = GeneralMath.map(args.y, 0, 200, 30, -30);
//Debug.WriteLine("Pixy: bearing=" + bearing + " inclination: " + inclination);
lock (currentSensorsDataLock)
{
SensorsData sensorsData = new SensorsData(this.currentSensorsData);
sensorsData.TargetingCameraBearingDegrees = bearing;
sensorsData.TargetingCameraInclinationDegrees = inclination;
sensorsData.TargetingCameraTimestamp = args.timestamp;
//Debug.WriteLine(sensorsData.ToString());
this.currentSensorsData = sensorsData;
}
}
}
internal void ArduinoBrickGpsChanged(RobotAbstraction.IHardwareComponent sender)
{
IGps gps = (IGps)sender;
lock (currentSensorsDataLock)
{
SensorsData sensorsData = new SensorsData(this.currentSensorsData);
sensorsData.GpsLatitude = gps.Latitude;
sensorsData.GpsLongitude = gps.Longitude;
sensorsData.GpsAltitude = gps.Altitude;
sensorsData.GpsNsat = gps.GpsNsat;
sensorsData.GpsHdop = gps.GpsHdop;
sensorsData.FixAgeMs = gps.FixAgeMs;
sensorsData.GpsTimeUTC = gps.TimeUTC;
sensorsData.GpsFixType = gps.FixType;
//Debug.WriteLine(sensorsData.ToString());
this.currentSensorsData = sensorsData;
}
}
/// <summary>
/// we can create sensors here, but cannot send commands before bridge_CommunicationStarted is called in PluckyTheRobot
/// for example, encoder.Clear() will hang.
/// </summary>
public async Task InitSensors(CancellationTokenSource cts)
{
// see C:\Projects\Serializer_3_0\ExampleApps\AnalogSensorExample\AnalogSensorExample\Form1.cs
// Note: the Element board communicates at 19200 Baud, which is roughly 1.5 kbytes/sec
// Comm link is busy with motor commands and has to be responsive to encoders, for odometry to work.
// Sensors must carefully adjust their demands by setting UpdateFrequency and Enabled properties.
// *********************** Parking Sonar:
SensorPose spParkingSonar = new SensorPose() { XMeters = 0.0d, YMeters = 0.0d, ThetaRadians = 0.0d };
ParkingSonar = RangerSensorFactory.produceRangerSensor(RangerSensorFactoryProducts.RangerSensorParkingSonar, "ParkingSonar", spParkingSonar,
hardwareBrick, rangersSamplingIntervalMs);
ParkingSonar.distanceChangedEvent += new EventHandler<RangerSensorEventArgs>(RangerDistanceChangedEvent);
RangerSensors.Add(ParkingSonar.Name, ParkingSonar);
// *********************** wheel encoders - roughly XXX ticks per wheel revolution, XXX ticks per meter.
encoderLeft = CreateWheelEncoder(hardwareBrick, WheelEncoderId.Encoder2, (int)mainLoopCycleMs, encodersSensitivityThresholdTicks);
encoderRight = CreateWheelEncoder(hardwareBrick, WheelEncoderId.Encoder1, (int)mainLoopCycleMs, encodersSensitivityThresholdTicks);
Ahrs = hardwareBrick.produceAhrs(0x00, AhrsSamplingIntervalMs, AhrsSensitivityThreshold);
Ahrs.ValuesChanged += new HardwareComponentEventHandler(Ahrs_ValuesChanged);
await RPiCameraSensor.Open(cts);
RPiCameraSensor.TargetingCameraTargetsChanged += RPiCameraSensor_TargetsChanged;
batteryVoltage = CreateBatteryVoltageMeter(hardwareBrick, batterySamplingIntervalMs, batterySensitivityThresholdVolts);
batteryVoltage.Enabled = true; // slow update rate, leave it turned on
SonarsEnabled = true;
EncodersEnabled = true;
CompassEnabled = true;
RPiCameraEnabled = true;
currentSensorsData = new SensorsData() { RangerSensors = this.RangerSensors };
}
internal void ArduinoBrickOdometryChanged(RobotAbstraction.IHardwareComponent sender)
{
IOdometry odom = (IOdometry)sender;
//odom.XMeters;
//odom.YMeters;
//odom.ThetaRadians;
lock (currentSensorsDataLock)
{
SensorsData sensorsData = new SensorsData(this.currentSensorsData);
sensorsData.WheelEncoderLeftTicks = odom.LDistanceTicks;
sensorsData.WheelEncoderRightTicks = odom.RDistanceTicks;
//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 RangerDistanceChangedEvent(object sender, RangerSensorEventArgs e)
{
//Debug.WriteLine("Ranger: " + e.Name + " RangeMeters=" + e.RangeMeters);
lock (currentSensorsDataLock)
{
SensorsData sensorsData = new SensorsData(this.currentSensorsData);
switch (e.Name)
{
case "ParkingSonar":
sensorsData.RangerFrontRightMeters = e.RangeMeters[0];
sensorsData.RangerFrontLeftMeters = e.RangeMeters[1];
sensorsData.RangerRearRightMeters = e.RangeMeters[2];
sensorsData.RangerRearLeftMeters = e.RangeMeters[3];
sensorsData.RangerFrontRightMetersTimestamp = sensorsData.RangerFrontLeftMetersTimestamp =
sensorsData.RangerRearRightMetersTimestamp = sensorsData.RangerRearLeftMetersTimestamp = e.TimeTicks;
break;
default:
throw new NotImplementedException("Error: RangerDistanceChangedEvent: unknown name " + e.Name);
}
//Debug.WriteLine(sensorsData.ToString());
this.currentSensorsData = sensorsData;
}
}
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;
}
}
private void RPiCameraSensor_TargetsChanged(object sender, TargetingCameraEventArgs args)
{
// Raspberry Pi based camera sensor works under Wheezy and uses OpenCV and Python to process
// 240x320 frames and select areas with yellow color. Bearing, inclination and size of blobs is then
// reported over HTTP to RPiCamera (derived from HttpServerBase). Frequency is around 10 FPS.
//Debug.WriteLine("RPi Camera Event: " + args);
// On Raspberry Pi:
// pixy.blocks[i].signature The signature number of the detected object (1-7)
// pixy.blocks[i].x The x location of the center of the detected object (0 to 319)
// pixy.blocks[i].y The y location of the center of the detected object (0 to 199)
// pixy.blocks[i].width The width of the detected object (1 to 320)
// pixy.blocks[i].height The height of the detected object (1 to 200)
// Field of view:
// goal 45 degrees left x=10
// middle x=160
// goal 45 degrees right x=310
//
// goal 30 degrees up y=10
// middle y=90
// goal 30 degrees down y=190
//
if (args.width * args.height > 500) // only large objects count
{
int bearing = GeneralMath.map(args.x, 0, 320, 45, -45);
int inclination = GeneralMath.map(args.y, 0, 200, 30, -30);
//Debug.WriteLine("RPi: bearing=" + bearing + " inclination: " + inclination);
lock (currentSensorsDataLock)
{
SensorsData sensorsData = new SensorsData(this.currentSensorsData);
sensorsData.TargetingCameraBearingDegrees = bearing;
sensorsData.TargetingCameraInclinationDegrees = inclination;
sensorsData.TargetingCameraTimestamp = args.timestamp;
//Debug.WriteLine(sensorsData.ToString());
this.currentSensorsData = sensorsData;
}
}
}
