public TelemetryReceivedEventArgs(TelemetryFeedback telemetry)
{
if(telemetry == null)
throw new ArgumentNullException("telemetry");
this.telemetry = telemetry;
}
public TelemetryFeedbackArgs(TelemetryFeedback updatedState)
{
if (updatedState == null)
throw new ArgumentNullException("Updated state is null");
this.state = updatedState;
}
public static TelemetryFeedback Decode(byte[] bytes)
{
int position = 0;
if (bytes == null)
throw new ArgumentNullException("Telemetry byte string is empty.");
if (bytes.Length < MIN_SIZE)
throw new ArgumentException("Incorrect size", "bytes");
TelemetryFeedback feedback = new TelemetryFeedback();
//Doubles
feedback.DriveBatteryVoltage = BitConverter.ToDouble(bytes, position);
position += sizeof(double);
feedback.CleanBatteryVoltage = BitConverter.ToDouble(bytes, position);
position += sizeof(double);
feedback.PowerUsed = BitConverter.ToDouble(bytes, position);
position += sizeof(double);
feedback.ScoopPitchAngle = BitConverter.ToDouble(bytes, position);
position += sizeof(double);
feedback.ArmSwingAngle = BitConverter.ToDouble(bytes, position);
position += sizeof(double);
feedback.ArmMotorAmps = BitConverter.ToDouble(bytes, position);
position += sizeof(double);
feedback.BinLeftMotorAmps = BitConverter.ToDouble(bytes, position);
position += sizeof(double);
feedback.BinRightMotorAmps = BitConverter.ToDouble(bytes, position);
position += sizeof(double);
feedback.TiltX = BitConverter.ToDouble(bytes, position);
position += sizeof(double);
feedback.TiltY = BitConverter.ToDouble(bytes, position);
position += sizeof(double);
feedback.RearProximityLeft = BitConverter.ToDouble(bytes, position);
position += sizeof(double);
feedback.RearProximityRight = BitConverter.ToDouble(bytes, position);
position += sizeof(double);
feedback.X = BitConverter.ToDouble(bytes, position);
position += sizeof(double);
feedback.Y = BitConverter.ToDouble(bytes, position);
position += sizeof(double);
feedback.Psi = BitConverter.ToDouble(bytes, position);
position += sizeof(double);
feedback.Confidence = BitConverter.ToDouble(bytes, position);
position += sizeof(double);
//Bools
feedback.ScoopLowerLimitSwitchDepressed = BitConverter.ToBoolean(bytes, position);
position += sizeof(bool);
feedback.ScoopUpperLimitSwitchDepressed = BitConverter.ToBoolean(bytes, position);
position += sizeof(bool);
feedback.BinLowerSwitchDepressed = BitConverter.ToBoolean(bytes, position);
position += sizeof(bool);
feedback.BinUpperSwitchDepressed = BitConverter.ToBoolean(bytes, position);
position += sizeof(bool);
//Ints
feedback.State = BitConverter.ToInt32(bytes, position);
position += sizeof(int);
feedback.FrontRangeFinderDataLength = BitConverter.ToInt32(bytes, position);
position += sizeof(int);
feedback.RearRangeFinderDataLength = BitConverter.ToInt32(bytes, position);
position += sizeof(int);
//process said length worth of data
feedback.FrontRangeFinderData = new double[feedback.FrontRangeFinderDataLength];
for (int i = 0; i < feedback.FrontRangeFinderDataLength; ++i)
{
feedback.FrontRangeFinderData[i] = BitConverter.ToDouble(bytes, position);
position += sizeof(double);
}
feedback.RearRangeFinderData = new double[feedback.RearRangeFinderDataLength];
for (int i = 0; i < feedback.RearRangeFinderDataLength; ++i)
{
feedback.RearRangeFinderData[i] = BitConverter.ToDouble(bytes, position);
position += sizeof(double);
}
return feedback;
}
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();
}
static void telemetryHandler_TelemetryFeedbackProcessed(object sender, Comms.TelemetryEncoding.TelemetryFeedbackArgs e)
{
// Obtain measurements and copy to feedback object
feedback = (TelemetryFeedback)e.UpdatedState.Clone();
}
