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