Exemplo n.º 1
0
        /// <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 SensorsDataPlucky()
            {
                RangerSensors = this.RangerSensors
            };
        }
Exemplo n.º 2
0
        /// <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
            };
        }