public AutonomyHandler(AutonomyConfiguration configuration, ref TelemetryHandler telemetryHandler)
{
// Assign fields
this.configuration = configuration;
// Create localization logic
localization = new MonteCarloLocalization(configuration);
// Create robot
robot = new Robot(configuration);
// Create stopwatch
stopwatch = new Stopwatch();
// Add telemetry callback
telemetryHandler.TelemetryFeedbackProcessed += telemetryHandler_TelemetryFeedbackProcessed;
// Create timer
timer = new Timer(configuration.Interval);
timer.Elapsed += timer_Elapsed;
// Create algorithms
depositionAlgorithm = new Deposition(configuration);
lowRiskLocalizationAlgorithm = new LowRiskLocalization(configuration);
// Create static state
staticOutput = new Dictionary<CommandFields, short>();
staticOutput[Comms.CommandEncoding.CommandFields.Mode] = 1;
staticOutput[Comms.CommandEncoding.CommandFields.FrontCameraState] = 0;
staticOutput[Comms.CommandEncoding.CommandFields.BucketCameraState] = 0;
staticOutput[Comms.CommandEncoding.CommandFields.RearCameraState] = 0;
}
private void UpdateTelemetry(MonteCarloLocalization localization)
{
// Update telemetry
Dictionary<Configuration.Telemetry, int> dictionary = new Dictionary<Configuration.Telemetry, int>();
//dictionary[Configuration.Telemetry.LocalizationX] = (int)(localization.Tracker.X * 10.0d); // Convert for double to int -> Like RoboteQ
//dictionary[Configuration.Telemetry.LocalizationY] = (int)(localization.Tracker.Y * 10.0d); // Convert for double to int -> Like RoboteQ
//dictionary[Configuration.Telemetry.LocalizationPsi] = (int)(localization.Tracker.Angle * 10.0d); // Convert for double to int -> Like RoboteQ
//dictionary[Configuration.Telemetry.LocalizationConfidence] = (int)(100.0 * localization.Confidence); // 0 = No confidence, 100 = Complete convergence/good confidence
//dictionary[Configuration.Telemetry.LocalizationState] = (int)state;
dictionary[Configuration.Telemetry.LocalizationX] = 10;
dictionary[Configuration.Telemetry.LocalizationY] = 10;
dictionary[Configuration.Telemetry.LocalizationPsi] = 90;
dictionary[Configuration.Telemetry.LocalizationConfidence] = 100;
dictionary[Configuration.Telemetry.LocalizationState] = (int)state;
OnTelemetryUpdated(new TelemetryEventArgs((int)configuration.Interval, dictionary));
}