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;
        }
Exemple #2
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();
        }