public RobotBrain(RobotSpecification specRobot)
{
m_specRobot = specRobot;
// Create comm link channels
m_Channels = new Dictionary<string, CommLink>();
foreach (SpecComm cs in specRobot.CommSpecs)
{
CommLink comm = null;
switch (cs.CommunicatorType)
{
case SpecComm.CommType.Serial:
comm = new SerialComm();
break;
case SpecComm.CommType.TCP:
comm = new TcpComm();
break;
}
comm.Initialize(cs.Name, specRobot.RobotID, cs.Parameters);
m_Channels.Add(cs.Name, comm);
// Only support for one telemetry channel
if (cs.HasTelemetry)
m_TelemetryChannel = cs.Name;
// Only support for one remote control channel
if (cs.HasRemoteControl)
m_RemCtlChannel = cs.Name;
}
// Load behavior stack
LoadBehaviorSet();
}
public RemCtlWnd(RobotSpecification specRobot, CommLink commLink)
{
InitializeComponent();
m_specRobot = specRobot;
m_CommLink = commLink;
}
public override void Initialize(RobotSpecification specRobot, ActivityLog logActivity)
{
base.Initialize(specRobot, logActivity);
if (specRobot.GeneralSpecs.ContainsKey("CruiseDistance"))
m_CruiseDistance = specRobot.GeneralSpecs["CruiseDistance"];
}
public virtual void Initialize(RobotSpecification specRobot, ActivityLog logActivity)
{
m_Name = this.GetType().Name;
m_Priority = specRobot.BehaviorSpecs[m_Name].Priority;
m_specRobot = specRobot;
m_logActivity = logActivity;
}
public override void Initialize(RobotSpecification specRobot, ActivityLog logActivity)
{
base.Initialize(specRobot, logActivity);
// Initialize Heading Hold PID
m_HhPID = new PID(1, 0, 0, 0, -80, 80);
if (specRobot.GeneralSpecs.ContainsKey("DesiredHeading"))
m_DesiredHeading = int.Parse(specRobot.GeneralSpecs["DesiredHeading"]);
else
m_DesiredHeading = -1;
}
public override void Initialize(RobotSpecification specRobot, ActivityLog logActivity)
{
base.Initialize(specRobot, logActivity);
m_Route = new List<NavOperator>();
m_InstPtr = 0;
m_IsFirst = true;
// Load route operators
foreach (SpecRoute sr in specRobot.RouteSpecs)
{
Type typeOp = Type.GetType("BehaviorSet_R7.NavOperators." + sr.OperatorName);
NavOperator op = (NavOperator)Activator.CreateInstance(typeOp);
op.Initialize(sr.OperatorName, logActivity, String2IntArray(sr.Parameters));
m_Route.Add(op);
}
}
private void ReceiveSensorDataTask(RobotSpecification specRobot)
{
while (true)
{
if (m_tsCancel.IsCancellationRequested)
{
m_LogEntries.AddEntry(new ActivityLogEntry("Sensor Task stopped"));
break;
}
else
{
if (m_RunSensors)
{
try
{
// Time telemetry request/reception
System.Diagnostics.Stopwatch sw = System.Diagnostics.Stopwatch.StartNew();
// Request and receive remote sensor data via telemetry channel
SensorRepository sr = new SensorRepository(specRobot);
m_Channels[m_TelemetryChannel].Send("T1");
string data = m_Channels[m_TelemetryChannel].ReadLine();
sw.Stop();
// Parse received sensor data
sr.ParseSensorData(data, specRobot.SensorSpecs.Count);
if (sr.IsValid)
{
// Add sensor repository to behavior/arbitrate queue
if (m_SensorReadings.TryAdd(sr, 1, m_tsCancel.Token))
{
sr.AcquisitionTime = sw.ElapsedMilliseconds;
sr.ID = m_SensorRepID;
m_SensorRepID++;
}
else
{
// Behaviors have not consumed previous sensor data so remove it and get newer
m_LogEntries.AddEntry(new ActivityLogEntry("Sensor data not used."));
if (m_SensorReadings.TryTake(out sr, 1, m_tsCancel.Token))
m_LogEntries.AddEntry(new ActivityLogEntry("Old sensor data removed!"));
}
}
else
{
// Lost communications with the robot
m_LogEntries.AddEntry(new ActivityLogEntry(ActivityLogEntry.LogEntryType.Error, "No communications with the robot!", null));
m_LogEntries.AddEntry(new ActivityLogEntry("RX Data = " + data));
}
m_LogEntries.AddEntry(new ActivityLogEntry("Sensor Acquisition Time", sw.ElapsedMilliseconds));
}
catch (Exception ex)
{
m_LogEntries.AddEntry(new ActivityLogEntry("Exception in Sensor Task: " + ex.Message));
}
}
}
}
}
private void ProcessBehaviorsTask(RobotSpecification specRobot)
{
while (true)
{
if (m_tsCancel.IsCancellationRequested)
{
m_LogEntries.AddEntry(new ActivityLogEntry("Behavior Task stopped"));
break;
}
else
{
try
{
SensorRepository sr;
if (m_SensorReadings.TryTake(out sr, -1, m_tsCancel.Token))
{
m_LogEntries.AddEntry(new ActivityLogEntry(ActivityLogEntry.LogEntryType.SensorData, "Get sensor data", sr));
// Run each Behavior in order of priority
if (m_RunBehaviors)
{
foreach (Behavior behavior in m_Behaviors)
{
// Execute the behavior and enqueue any requests it may have generated
RequestQueue requests = behavior.Execute(sr, m_WinningRequests);
if (requests != null && requests.Count > 0)
m_RequestQueueQueue.Enqueue(requests);
}
}
// Arbitrate and send winning behavior's requests
if (m_RequestQueueQueue.Count > 0)
{
// Since behaviors are executed in priority order the highest will always be the first in the request queue queue
m_WinningRequests = m_RequestQueueQueue.Dequeue();
m_LastWinnerName = m_WinningRequests.BehaviorName;
// Send each request out the selected comm link
SendRequests(m_WinningRequests);
// Clear out the queue for the next time
m_RequestQueueQueue.Clear();
}
else
m_WinningRequests = null;
}
}
catch (Exception ex)
{
m_LogEntries.AddEntry(new ActivityLogEntry("Exception in Behavior Task: " + ex.Message));
}
}
}
}
private void miOpen_Click(object sender, RoutedEventArgs e)
{
string Filename = Properties.Settings.Default.LastSpecFile;
OpenFileDialog dlg = new OpenFileDialog();
try
{
dlg.InitialDirectory = System.IO.Path.GetDirectoryName(Filename);
dlg.FileName = System.IO.Path.GetFileName(Filename);
}
catch
{
dlg.InitialDirectory = Environment.CurrentDirectory;
dlg.FileName = "";
}
dlg.Filter = "XML files (*.xml)|*.xml|All files (*.*)|*.*";
dlg.FilterIndex = 1;
dlg.RestoreDirectory = true;
if (dlg.ShowDialog() == true)
{
btnStop_Click(sender, e);
Properties.Settings.Default.LastSpecFile = dlg.FileName;
// Create the robot brain!
m_specRobot = RobotSpecification.Load(dlg.FileName);
m_Robot = new RobotBrain(m_specRobot);
// Setup UI timer
if (m_timerUI == null)
{
m_timerUI = new DispatcherTimer();
m_timerUI.Interval = TimeSpan.FromMilliseconds(1);
m_timerUI.Tick += new EventHandler(UpdateUI);
m_timerUI.Start();
}
this.Title = m_Title + " - " + System.IO.Path.GetFileName(dlg.FileName);
}
}
private RobotSpecification BuildRobotSpec()
{
RobotSpecification specRobot = new RobotSpecification();
specRobot.RobotID = "R7";
specRobot.BehaviorsPath = @"C:\Devel\EmergeStudio\BehaviorSet-R7\bin\Debug";
SpecComm cs1 = new SpecComm() { Name = "Drive", CommunicatorType = SpecComm.CommType.Serial, HasTelemetry = true, HasRemoteControl = true };
cs1.Parameters.Add("COMPORT", "COM25");
cs1.Parameters.Add("BAUD", "9600");
specRobot.CommSpecs.Add(cs1);
specRobot.SensorSpecs.Add(new SpecSensor { Name = "DistFwd", Position = 0, RetType = typeof(int), Plot = true });
specRobot.SensorSpecs.Add(new SpecSensor { Name = "DistR30", Position = 1, RetType = typeof(int), Plot = true });
specRobot.SensorSpecs.Add(new SpecSensor { Name = "DistL30", Position = 2, RetType = typeof(int), Plot = true });
specRobot.SensorSpecs.Add(new SpecSensor { Name = "DistL90", Position = 3, RetType = typeof(int) });
specRobot.SensorSpecs.Add(new SpecSensor { Name = "Heading", Position = 4, RetType = typeof(int) });
specRobot.SensorSpecs.Add(new SpecSensor { Name = "Encoder1", Position = 5, RetType = typeof(int), Plot = true });
specRobot.SensorSpecs.Add(new SpecSensor { Name = "Encoder2", Position = 6, RetType = typeof(int) });
specRobot.SensorSpecs.Add(new SpecSensor { Name = "Speed", Position = 7, RetType = typeof(int) });
specRobot.SensorSpecs.Add(new SpecSensor { Name = "Direction", Position = 8, RetType = typeof(int) });
specRobot.SensorSpecs.Add(new SpecSensor { Name = "IsPower", Position = 9, RetType = typeof(bool) });
specRobot.SensorSpecs.Add(new SpecSensor { Name = "HasArrived", Position = 10, RetType = typeof(bool) });
specRobot.SensorSpecs.Add(new SpecSensor { Name = "ArmBase", Position = 11, RetType = typeof(int) });
specRobot.SensorSpecs.Add(new SpecSensor { Name = "ArmElbow", Position = 12, RetType = typeof(int) });
specRobot.SensorSpecs.Add(new SpecSensor { Name = "ArmWrist", Position = 13, RetType = typeof(int) });
specRobot.SensorSpecs.Add(new SpecSensor { Name = "ArmWristRot", Position = 14, RetType = typeof(int) });
specRobot.SensorSpecs.Add(new SpecSensor { Name = "ArmGripper", Position = 15, RetType = typeof(int) });
SpecBehavior bs = new SpecBehavior() { Name = "Blocked", Priority = 1 };
bs.Parameters.Add("Parameter1", "abc");
specRobot.BehaviorSpecs.Add("Blocked", bs);
specRobot.BehaviorSpecs.Add("Avoid", new SpecBehavior() { Name = "Avoid", Priority = 2 });
specRobot.BehaviorSpecs.Add("Cruise", new SpecBehavior() { Name = "Cruise", Priority = 3 });
specRobot.HaltRequests.Add(new Request() { Name = "Halt/Reset", Channel = "Drive", Command = "EH" });
specRobot.HaltRequests.Add(new Request() { Name = "Power Off", Channel = "Drive", Command = "p1" });
specRobot.GeneralSpecs.Add("CruiseDistance", "40");
specRobot.ControlSpecs.Add(new SpecControl() { ControlName = "btnPower2", IsEnabled = false });
specRobot.ControlSpecs.Add(new SpecControl() { ControlName = "btnPower3", IsEnabled = false });
specRobot.ControlSpecs.Add(new SpecControl() { ControlName = "btnFunc1", IsEnabled = false });
specRobot.ControlSpecs.Add(new SpecControl() { ControlName = "btnFunc2", IsEnabled = false });
specRobot.ControlSpecs.Add(new SpecControl() { ControlName = "btnFunc3", IsEnabled = false });
specRobot.ControlSpecs.Add(new SpecControl() { ControlName = "btnVidUp", IsEnabled = false });
specRobot.ControlSpecs.Add(new SpecControl() { ControlName = "btnVidDown", IsEnabled = false });
specRobot.ControlSpecs.Add(new SpecControl() { ControlName = "btnVidLeft", IsEnabled = false });
specRobot.ControlSpecs.Add(new SpecControl() { ControlName = "btnVidRight", IsEnabled = false });
specRobot.ControlSpecs.Add(new SpecControl() { ControlName = "btnVidCenter", IsEnabled = false });
specRobot.ControlSpecs.Add(new SpecControl() { ControlName = "btnVidScan", IsEnabled = false });
// Save it
// specRobot.Save("Test.xml");
return specRobot;
}
internal SensorRepository(RobotSpecification specRobot)
{
m_specRobot = specRobot;
}
public override void Initialize(RobotSpecification specRobot, ActivityLog logActivity)
{
base.Initialize(specRobot, logActivity);
}
