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; }
static void Main(string[] args) { //load config file from arguments foreach (string str in args) { char[] separator = { '=' }; string[] arg_pair = str.Split(separator, 2, StringSplitOptions.RemoveEmptyEntries); switch (str) { case "--noAutonomy": useAutonomy = false; break; case "--noRangeFinder": useRangeFinders = false; break; case "--noRoboteQ": useRoboteQs = false; break; case "--noWebcam": useWebcams = false; break; case "--noServoController": useServoController = false; break; default: break; } if (arg_pair.Length < 2) continue; switch (arg_pair[0]) { case "--config": //load file... XmlSerializer deserializer = new System.Xml.Serialization.XmlSerializer(typeof(RCUConfiguration)); try { using (StreamReader file_reader = new StreamReader(arg_pair[1])) { configuration = (RCUConfiguration)deserializer.Deserialize(file_reader); } } catch (Exception ex) { Console.WriteLine("Error opening configuration file: " + ex.Message); } break; default: break; } } try { if (configuration == null) throw new Exception("None or invalid configuration specified"); mode = Mode.Manual; feedback = new TelemetryFeedback(); stopwatch = new Stopwatch(); telemetryHandler = new Telemetry.TelemetryHandler(configuration.TelemetryConfiguration, configuration.OCU_IP_Address); telemetryHandler.Activate(); // Load RoboteQs if (useRoboteQs) { foreach (var roboteq_config in configuration.RoboteqConfigurations) roboteqs.Add(new Controllers.RoboteQ(roboteq_config)); foreach (Controllers.RoboteQ roboteq in roboteqs) { telemetryHandler.AddProvider(roboteq); roboteq.Activate(); } } // Load rangefinders if (useRangeFinders) { frontRangeFinder = new RangeFinder(configuration.FrontRangeFinder); frontRangeFinder.Activate(); rearRangeFinder = new RangeFinder(configuration.RearRangeFinder); rearRangeFinder.Activate(); telemetryHandler.AddRangeFinder(Telemetry.TelemetryHandler.RangeFinderLocation.Front, frontRangeFinder); telemetryHandler.AddRangeFinder(Telemetry.TelemetryHandler.RangeFinderLocation.Rear, rearRangeFinder); } if (useWebcams) { frontCam = new Webcam(configuration.OCU_IP_Address, configuration.FrontCameraConfiguration); Thread.Sleep(200); frontCam.Activate(); Thread.Sleep(200); bucketCam = new Webcam(configuration.OCU_IP_Address, configuration.BucketCameraConfiguration); Thread.Sleep(200); bucketCam.Activate(); Thread.Sleep(200); rearCam = new Webcam(configuration.OCU_IP_Address, configuration.RearCameraConfiguration); Thread.Sleep(200); rearCam.Activate(); Thread.Sleep(200); } // Create servo controller if (useServoController) { servo = new PhidgetServo(configuration.PhidgetConfiguration); servo.Activate(); } if (useAutonomy) { // Start autonomy logic autonomy = new AutonomyHandler(configuration.AutonomyConfiguration, ref telemetryHandler); autonomy.AutonomyUpdated += autonomy_AutonomyUpdated; autonomy.Activate(); telemetryHandler.AddProvider(autonomy); } //start listening... receiver = new Comms.UDP_Receiver(configuration.CommandPort); receiver.DataReceived += new EventHandler<Comms.DataArgs>(receiver_DataReceived); receiver.Activate(); telemetryHandler.TelemetryFeedbackProcessed += telemetryHandler_TelemetryFeedbackProcessed; //packet handler stateProcessor = new Thread(new ThreadStart(StateProcessorDoWork)); stateProcessor.Start(); } catch (Exception ex) { Console.WriteLine("Error during startup: " + ex.Message); Teardown(); } while (true) { try { Console.WriteLine("Enter 'exit' to shutdown"); string input = Console.ReadLine().ToLower(); if (input.Contains("exit")) { break; } } catch (Exception) { } } Teardown(); // Pause Console.WriteLine("Press any key to close"); Console.ReadKey(); }