void Timer_Tick(object sender, System.Timers.ElapsedEventArgs args)
{
try
{
Application.Current.Dispatcher.Invoke(() =>
{
try
{
this.Visibility = ConsoleDisplay.DisplayMap ? System.Windows.Visibility.Visible : System.Windows.Visibility.Hidden;
}
catch (Exception)
{ }
if (!ConsoleDisplay.DisplayMap)
{
return;
}
if (PreviousBoundary == null || PreviousBoundary != ConsoleDisplay.ugv.Boundary)
{
PreviousBoundary = ConsoleDisplay.ugv.Boundary;
map.DefineBoundary(ConsoleDisplay.ugv.Boundary);
}
if (PreviousSafeZone == null || PreviousSafeZone != ConsoleDisplay.ugv.SafeZone)
{
PreviousSafeZone = ConsoleDisplay.ugv.SafeZone;
map.DefineSafeZone(ConsoleDisplay.ugv.SafeZone);
}
UGV.Core.Navigation.WayPoint CurrentLocation = new UGV.Core.Navigation.WayPoint(ConsoleDisplay.ugv.Latitude, ConsoleDisplay.ugv.Longitude, 0);
map.DefineVisionPath(CurrentLocation, ConsoleDisplay.ugv.VisionWaypoints);
map.DefineSearchPath(CurrentLocation, ConsoleDisplay.ugv.Waypoints.ToList());
//map.DefineTrackPoint(CurrentLocation);
map.DefineHeading(CurrentLocation, ConsoleDisplay.ugv.Heading, 1000000.0 / Math.Pow(2, map.ZoomLevel));
if (ConsoleDisplay.AlwaysCenter)
{
map.CenterAt(CurrentLocation.Lat, CurrentLocation.Long);
}
if (ConsoleDisplay.ugv.TargetLockedLocation != null)
{
map.DefineTargetPoint(ConsoleDisplay.ugv.TargetLockedLocation);
}
else if (ConsoleDisplay.ugv.VisionTargets.Count > 0)
{
var target = ConsoleDisplay.ugv.VisionTargets[0];
map.DefineTargetPoint(target.Lat, target.Long);
}
else
{
map.RemoveTargetPoint();
}
if (ConsoleDisplay.ZoomIn)
{
map.ZoomLevel += 0.1;
ConsoleDisplay.ZoomIn = false;
}
if (ConsoleDisplay.ZoomOut)
{
map.ZoomLevel -= 0.1;
ConsoleDisplay.ZoomOut = false;
}
});
}
catch (Exception)
{ }
}
public static void Main(string[] args)
#endif
{
//System.Diagnostics.Debugger.Break();
//Set the window size, it's in chars for the width and lines for the height, not pixels.
Console.SetWindowSize(100, 32);
//initialize
try {
ugv = new NGCP.UGV.UGV();
}
catch (Exception e)
{
Console.WriteLine(e.Message);
}
timer = new System.Timers.Timer();
timer.Interval = 200;
timer.Elapsed += new System.Timers.ElapsedEventHandler(TimerTick);
//read configuration
Dictionary <string, string> Settings = new Dictionary <string, string>
{
{ "UseFPGA", "False" },
{ "FPGAPort", "COM7" },
{ "FPGABaud", "9600" },
{ "UseXBeeComm", "False" },
{ "UseCommProtocol", "False" },
{ "CommPort", "COM3" },
{ "CommBaud", "57600" },
{ "CommNode", "5" },
{ "CommAddresses", "1 0013A2004067E4AE 100 0013A20040A814FD" },
{ "UseNav", "False" },
{ "NavPort", "COM9" },
{ "NavBaud", "57600" },
{ "UseArm", "False" },
{ "ArmPort", "COM11" },
{ "ArmBaud", "8000" },
{ "UseVision", "True" },
{ "VisionHostIP", "127.0.0.1" },
{ "VisionHostPort", "8009" },
{ "UseCamera", "False" },
{ "CameraHostPort", "5501" },
{ "VisionTargetIP", "127.0.0.1" },
{ "VisionTargetPort", "5501" }
};
try
{
//read ini file
ReadIni(Settings);
}
catch (UnauthorizedAccessException uae)
{
Console.WriteLine("An Unauthorized Access Exception happened:");
Console.WriteLine("Please either remove the existing setting in registry: " + RegPath + " or start this code as admin.");
Console.WriteLine("Exception: " + uae.ToString());
Console.ReadLine();
return;
}
//read configuration
ugv.Settings.UseFPGA = Boolean.Parse(Settings["UseFPGA"]);
ugv.Settings.UseXBeeComm = Boolean.Parse(Settings["UseXBeeComm"]);
ugv.Settings.UseCommProtocol = Boolean.Parse(Settings["UseCommProtocol"]);
ugv.Settings.UseNav = Boolean.Parse(Settings["UseNav"]);
ugv.Settings.UseVision = Boolean.Parse(Settings["UseVision"]);
ugv.Settings.UseCamera = Boolean.Parse(Settings["UseCamera"]);
ugv.Settings.FPGAPort = Settings["FPGAPort"];
ugv.Settings.FPGABaud = Convert.ToInt32(Settings["FPGABaud"]);
ugv.Settings.CommPort = Settings["CommPort"];
ugv.Settings.CommBaud = Convert.ToInt32(Settings["CommBaud"]);
ugv.Settings.CommNode = Convert.ToInt32(Settings["CommNode"]);
ugv.Settings.CommAddresses = (Settings["CommAddresses"]);
ugv.Settings.NavPort = Settings["NavPort"];
ugv.Settings.NavBaud = Convert.ToInt32(Settings["NavBaud"]);
ugv.Settings.ArmPort = Settings["ArmPort"];
ugv.Settings.ArmBaud = Convert.ToInt32(Settings["ArmBaud"]);
ugv.Settings.UseArm = Boolean.Parse(Settings["UseArm"]);
ugv.Settings.VisionHostIP = Settings["VisionHostIP"];
ugv.Settings.VisionHostPort = Convert.ToInt32(Settings["VisionHostPort"]);
ugv.Settings.CameraHostPort = Convert.ToInt32(Settings["CameraHostPort"]);
ugv.Settings.VisionTargetIP = Settings["VisionTargetIP"];
ugv.Settings.VisionTargetPort = Convert.ToInt32(Settings["VisionTargetPort"]);
ugv.Settings.DriveMode = NGCP.UGV.UGV.DriveMode.LocalControl;
bool DirectStart = false;
//wait for s to start
ConsoleKeyInfo keyInfo;
if (!DirectStart)
{
do
{
//get port names
List <string> PortNames = SerialPort.GetPortNames().ToList();
Console.Clear();
//show config
Console.WriteLine("Please Verify setting before start:");
foreach (var kv in Settings)
{
//check port exist
string exist = PortNames.Contains(kv.Value) ? " - Port Exist" : "";
Console.WriteLine("\t{0,-20} = \t{1,-10}\t{2,-10}", kv.Key, kv.Value, exist);
//Console.WriteLine("\t" + kv.Key + " = \t" + kv.Value + "\t" + exist);
}
Console.WriteLine("Dynamic Library Info:");
Console.WriteLine(GetDllInfo(typeof(UGV.Core.IO.Link)));
Console.WriteLine(GetDllInfo(typeof(NGCP.UGV.UGV)));
Console.WriteLine("Hit S to start...");
keyInfo = Console.ReadKey();
}while (keyInfo.Key != ConsoleKey.S);
}
//Logging stuff
#if logging
System.Timers.Timer logTimer = new System.Timers.Timer(1000);
logTimer.Elapsed += new System.Timers.ElapsedEventHandler(LogTick);
#endif
//start ugv
try
{
ugv.Start();
timer.Start();
#if logging
logTimer.Start();
#endif
}
catch (Exception ex)
{
timer.Stop();
Console.WriteLine("Cannot Start UGV:\n\t" + ex.ToString());
}
#region Waypoint and Boundary Definition
UGV.Core.Navigation.WayPoint WaypointA = new UGV.Core.Navigation.WayPoint(34.059298, -117.821791, 0);
UGV.Core.Navigation.WayPoint WaypointB = new UGV.Core.Navigation.WayPoint(34.059326, -117.822056, 0);
UGV.Core.Navigation.WayPoint WaypointC = new UGV.Core.Navigation.WayPoint(34.059212, -117.821988, 0);
//UGV.Core.Navigation.WayPoint WaypointB = new UGV.Core.Navigation.WayPoint(34.0590760, -117.8213999, 0);
/* UGV.Core.Navigation.WayPoint WaypointC = new UGV.Core.Navigation.WayPoint(34.0590225, -117.8216451, 0);
* UGV.Core.Navigation.WayPoint WaypointD = new UGV.Core.Navigation.WayPoint(34.0587586, -117.8218449, 0);
*/
ugv.Waypoints.Enqueue(WaypointA);
//ugv.Waypoints.Enqueue(WaypointB);
//ugv.Waypoints.Enqueue(WaypointC);
////ugv.Waypoints.Enqueue(WaypointD);
//ugv.TargetWaypoint = WaypointC;
List <UGV.Core.Navigation.WayPoint> KLotBoundary = new List <UGV.Core.Navigation.WayPoint>()
{
new UGV.Core.Navigation.WayPoint(34.05114, -117.81804, 0), // 34.05920, -117.82170, 0),
new UGV.Core.Navigation.WayPoint(34.05087, -117.81746, 0), //34.05940, -117.82125, 0),
new UGV.Core.Navigation.WayPoint(34.05107, -117.81746, 0), //34.05888, -117.82147, 0)
new UGV.Core.Navigation.WayPoint(34.05134, -117.81804, 0) //34.05900, -117.82115, 0),
};
List <UGV.Core.Navigation.WayPoint> EMBoundary = new List <UGV.Core.Navigation.WayPoint>()
{
new UGV.Core.Navigation.WayPoint(34.05920, -117.82170, 0),
new UGV.Core.Navigation.WayPoint(34.05940, -117.82125, 0),
new UGV.Core.Navigation.WayPoint(34.05900, -117.82115, 0),
new UGV.Core.Navigation.WayPoint(34.05888, -117.82147, 0)
};
List <UGV.Core.Navigation.WayPoint> SLOBoundary = new List <UGV.Core.Navigation.WayPoint>()
{
new UGV.Core.Navigation.WayPoint(35.328238, -120.752526, 0),
new UGV.Core.Navigation.WayPoint(35.328314, -120.752398, 0),
new UGV.Core.Navigation.WayPoint(35.327938, -120.752006, 0),
new UGV.Core.Navigation.WayPoint(35.328000, -120.751923, 0),
new UGV.Core.Navigation.WayPoint(35.329025, -120.752930, 0),
new UGV.Core.Navigation.WayPoint(35.328957, -120.753015, 0),
new UGV.Core.Navigation.WayPoint(35.328552, -120.752635, 0),
new UGV.Core.Navigation.WayPoint(35.328475, -120.752757, 0)
};
List <UGV.Core.Navigation.WayPoint> NGCWoodlandHillsBoundary = new List <UGV.Core.Navigation.WayPoint>()
{
new UGV.Core.Navigation.WayPoint(34.170803, -118.594467, 0),
new UGV.Core.Navigation.WayPoint(34.170806, -118.594306, 0),
new UGV.Core.Navigation.WayPoint(34.170136, -118.5943, 0),
new UGV.Core.Navigation.WayPoint(34.170136, -118.594461, 0),
};
List <UGV.Core.Navigation.WayPoint> GymBoundary = new List <UGV.Core.Navigation.WayPoint>()
{
new UGV.Core.Navigation.WayPoint(34.054943, -117.8196, 0),
new UGV.Core.Navigation.WayPoint(34.054648, -117.819438, 0),
new UGV.Core.Navigation.WayPoint(34.054428, -117.820116, 0),
new UGV.Core.Navigation.WayPoint(34.054706, -117.820269, 0)
};
List <UGV.Core.Navigation.WayPoint> QuadBoundary = new List <UGV.Core.Navigation.WayPoint>()
{
new UGV.Core.Navigation.WayPoint(34.058642, -117.823358, 0),
new UGV.Core.Navigation.WayPoint(34.058419, -117.823070, 0),
new UGV.Core.Navigation.WayPoint(34.058323, -117.823114, 0),
new UGV.Core.Navigation.WayPoint(34.058527, -117.823507, 0)
};
//debug safezone
List <UGV.Core.Navigation.WayPoint> KLotSafeZone = new List <UGV.Core.Navigation.WayPoint>()
{
new UGV.Core.Navigation.WayPoint(34.051103, -117.817660, 0), //34.05925, -117.82160, 0),
new UGV.Core.Navigation.WayPoint(34.051083, -117.817662, 0), //34.05935, -117.82135, 0),
new UGV.Core.Navigation.WayPoint(34.051111, -117.817727, 0), //34.05923, -117.82132, 0),
new UGV.Core.Navigation.WayPoint(34.051136, -117.817726, 0) //34.05913, -117.82157, 0)
};
List <UGV.Core.Navigation.WayPoint> EMSafeZone = new List <UGV.Core.Navigation.WayPoint>()
{
new UGV.Core.Navigation.WayPoint(34.05925, -117.82160, 0),
new UGV.Core.Navigation.WayPoint(34.05935, -117.82135, 0),
new UGV.Core.Navigation.WayPoint(34.05923, -117.82132, 0),
new UGV.Core.Navigation.WayPoint(34.05913, -117.82157, 0)
};
List <UGV.Core.Navigation.WayPoint> SLOSafeZone = new List <UGV.Core.Navigation.WayPoint>()
{
new UGV.Core.Navigation.WayPoint(35.328469, -120.752738, 0),
new UGV.Core.Navigation.WayPoint(35.328250, -120.752524, 0),
new UGV.Core.Navigation.WayPoint(35.328319, -120.752420, 0),
new UGV.Core.Navigation.WayPoint(35.328531, -120.752628, 0)
//new UGV.Core.Navigation.WayPoint(35.328238, -120.752526, 0),
//new UGV.Core.Navigation.WayPoint(35.328314, -120.752398, 0),
//new UGV.Core.Navigation.WayPoint(35.328552, -120.752635, 0),
//new UGV.Core.Navigation.WayPoint(35.328475, -120.752757, 0)
};
List <UGV.Core.Navigation.WayPoint> NGCWoodlandHillsSafeZone = new List <UGV.Core.Navigation.WayPoint>()
{
new UGV.Core.Navigation.WayPoint(34.170803, -118.594467, 0),
new UGV.Core.Navigation.WayPoint(34.170806, -118.594306, 0),
new UGV.Core.Navigation.WayPoint(34.170725, -118.594306, 0),
new UGV.Core.Navigation.WayPoint(34.170728, -118.594469, 0)
};
List <UGV.Core.Navigation.WayPoint> GymSafeZone = new List <UGV.Core.Navigation.WayPoint>()
{
new UGV.Core.Navigation.WayPoint(34.054943, -117.8196, 0),
new UGV.Core.Navigation.WayPoint(34.054648, -117.819438, 0),
new UGV.Core.Navigation.WayPoint(34.054643, -117.819513, 0),
new UGV.Core.Navigation.WayPoint(34.054914, -117.819681, 0)
};
List <UGV.Core.Navigation.WayPoint> QuadSafeZone = new List <UGV.Core.Navigation.WayPoint>()
{
new UGV.Core.Navigation.WayPoint(34.058527, -117.823507, 0),
new UGV.Core.Navigation.WayPoint(34.058643, -117.823362, 0),
new UGV.Core.Navigation.WayPoint(34.058591, -117.823289, 0),
new UGV.Core.Navigation.WayPoint(34.058510, -117.823405, 0)
};
var KLotDefault = new UGV.Core.Navigation.WayPoint(34.05107, -117.81746, 0);
var EMDefault = new UGV.Core.Navigation.WayPoint(34.05913, -117.82157, 0);
var SLODefault = new UGV.Core.Navigation.WayPoint(35.328455, -120.752294, 0);
var NGCWoodlandHillsDefault = new UGV.Core.Navigation.WayPoint(34.170764, 118.594394, 0);
var GymDefault = new UGV.Core.Navigation.WayPoint(34.054943, -117.8196, 0);
var QuadDefault = new UGV.Core.Navigation.WayPoint(34.058577, -117.823388, 0);
ugv.Settings.IMUGain = 0.5;
# endregion
