public MotorConfig(IAhrs ahrs, IControl roll, IControl pitch, IControl yaw)
{
_ahrs = ahrs;
_rollPid = roll;
_pitchPid = pitch;
_yawPid = yaw;
}
/// <summary>
/// Erstellt eine neue Instanz
/// </summary>
/// <param name="ahrs">Das AHRS (Attitude Heading Reference System)</param>
/// <param name="roll">Roll PID Regler</param>
/// <param name="pitch">Pitch PID Regler</param>
/// <param name="yaw">Yaw / Kurs PID Regler</param>
public PlusMotorConfig(IAhrs ahrs, IControl roll, IControl pitch, IControl yaw,
BrushlessMotor front, BrushlessMotor rear,
BrushlessMotor left, BrushlessMotor right)
: base(ahrs, roll, pitch, yaw)
{
_front = front;
_rear = rear;
_left = left;
_right = right;
}
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;
}
}
/// <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
};
}
/// <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 };
}
