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