/// <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"; if (PixyCameraSensor != null) { try { await PixyCameraSensor.Open(cts); PixyCameraSensor.TargetingCameraTargetsChanged += PixyCameraSensor_PixyCameraBlocksChanged; } catch (Exception exc) { speaker.Speak("Could not open Pixy Camera at " + PixyCameraSensor.ComPortName); PixyCameraSensor = null; } } 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; if (PixyCameraSensor != null) { PixyCameraSensor.Enabled = true; } currentSensorsData = new SensorsDataShorty() { RangerSensors = this.RangerSensors }; }