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; } } }