一、前言
在單元四中我們介紹了如何透過陀螺儀實現上帝視角運動模式,讓操控者更加容易操控車體,在這個單元中我們將使用 NetworkTables Library 與 Shuffleboard 應用程式,了解如何將各種感測器數值顯示在Shuffleboard、透過Shuffleboard控制 VMX 智慧移動平台。
以下分成幾個小節說明:
- Shuffleboard 介紹
- 透過程式建立Shuffleboard 元件
- 將超聲波感測器距離顯示在 Shuffleboard
- 透過Shuffleboard 按鈕控制 VMX
- 透過下拉選單選擇基礎或上帝視角運動模式
二、Shuffleboard 介紹
Shuffleboard 提供多種UI介面,讓使用者能將平台各種資訊透過區域網路傳到Shuffleboard上顯示,其中包含影像、各馬達的速度、編碼器距離,陀螺儀角度、滑動桿、按鈕等各種不同的UI,有了這些UI 後我們將可以透過區域網路以及NetworkTables 與 VMX 溝通,控制並觀察各個感測器的狀態,如下圖所示。
三、透過程式建立Shuffleboard 元件
(1)首先引入Shuffleboard 以及NetworkTableEntry Header file。
#include <frc/shuffleboard/Shuffleboard.h>#include <networktables/NetworkTableEntry.h> |
(2)使用frc、nt namespace。
using namespace frc;using namespace nt; |
(3)建立ShuffleboardTab 與NetworkTableEntry 物件
/** * Shuffleboard & NetworkTable */ShuffleboardTab & tab = Shuffleboard::GetTab(“Sensor”);NetworkTableEntry m_ntTemp;NetworkTableEntry m_ntHumi;NetworkTableEntry m_ntFSonic;NetworkTableEntry m_ntLSonic;NetworkTableEntry m_ntTcrt5000_R;NetworkTableEntry m_ntTcrt5000_M;NetworkTableEntry m_ntTcrt5000_L; |
(4)實例化NetworkTable,在呼叫Shuffleboard Add方法時帶入Table 名稱與初始值,呼叫Add方法後使用WithWidget 方法帶入元件樣板,最後透過GetEntry 方法取得此UI元件物件。
/** * instantiation Network Table */this->m_ntTemp = this->tab.Add(“Temperature”, 0).WithWidget(BuiltInWidgets::kTextView).GetEntry();this->m_ntHumi = this->tab.Add(“Humidity”, 0).WithWidget(BuiltInWidgets::kTextView).GetEntry();this->m_ntFSonic = this->tab.Add(“FSonic”, 0).WithWidget(BuiltInWidgets::kTextView).GetEntry();this->m_ntLSonic = this->tab.Add(“LSonic”, 0).WithWidget(BuiltInWidgets::kTextView).GetEntry();this->m_ntTcrt5000_R = this->tab.Add(“TCRT R”, 0).WithWidget(BuiltInWidgets::kTextView).GetEntry();this->m_ntTcrt5000_M = this->tab.Add(“TCRT M”, 0).WithWidget(BuiltInWidgets::kTextView).GetEntry();this->m_ntTcrt5000_L = this->tab.Add(“TCRT L”, 0).WithWidget(BuiltInWidgets::kTextView).GetEntry(); |
(5)元件樣板分為以下幾種:
- kTextView(顯示文字)
- kNumberSlider(顯示可動滑塊)
- kNumberBar(顯示僅可察看數字的條塊)
- kGraph(顯示數值圖表)
- kBooleanBox(顯示布林值方塊)
- kToggleButton(顯示Toggle 開關)
- kToggleSwitch(顯示Switch切換)
- kVoltageView(顯示電壓)
- kPowerDistributionPanel(顯示電源狀態)
- kComboBoxChooser(顯示下拉選單)
- kSplitButtonChooser(顯示帶有Toogle開關的下拉選單)
- kEncoder(顯示編碼器數值)
- kSpeedController(顯示速度控制器)
- kCommand(顯示命令按鈕,執行完成自動放開)
- kPIDCommand(顯示可調整參數的PID控制板,帶執行按鈕)
- kPIDController(顯示可調整參數的PID控制板)
- kAccelerometer(顯示加速度計)
- k3AxisAccelerometer(顯示三軸加速度計)
- kGyro(顯示0~360度的陀螺儀角度)
- kRelay(顯示各個模式的切換按鈕)
- kDifferentialDrive(顯示帶有差速驅動的馬達面板)
- kMecanumDrive(顯示各個麥克納姆輪的資訊,速度、方向等)
- kCameraStream(顯示串流影像)
四、將超音波感測器距離顯示在Shuffleboard
(1)在Shuffleboard 上建立溫溼度、超音波、循跡紅外線感測器等物件,如下圖所示。
(2)建立Shuffleboard、NetworkTable 物件,將Shuffleboard Table名稱設定為「Sensor」,如下圖所示。
(3)實例化 NetworkTable,如下圖所示。
(4)在Periodic 方法內將感測器數值發佈到各個對應Network 物件,Periodic 方法繼承自Subsystem 類別,Periodic 每20ms 將會執行一次,如下圖所示。
(5)Shuffleboard 應用程式顯示結果如下圖所示:
五、透過Shuffleboard 按鈕控制VMX 智慧移動平台
除了可以將 VMX 各種感測數值資料透過NetworkTables 發佈到Shuffleboard 上之外,VMX 也可以透過NetworkTables 讀取Shuffleboard 元件數值,並做出相對應動作,以下將介紹讀取Shuffleboard Toggle 按鈕狀態、TextView文字輸入,控制升降手臂移動位置。
(1)建立升降手臂subsystem,其中包含Servo、升降手臂輸出輸入IO、Shuffleboard、NetworkTable 物件,如下圖所示。
(2)在Periodic 方法內讀取Shuffleboard Enter 按鈕狀態,當按鈕被按下時將按鈕復歸,並且檢查數值並控制升降手臂位址與Servo 角度,如下圖所示。
六、透過下拉選單選擇基礎或上帝視角運動模式
在上一單元中,我們運用陀螺儀實作了上帝視角運動模式(以操控者視角為前方行進),這次我們加入一般的基礎控制模式(以車頭為前方行進),藉由Shuffleboard我們可以透過下拉選單元件來選擇要運行的模式,未來在有多種模式下只需要透過下拉選單選擇即可快速切換。
(1)在DriveSubsystem 加入SetBaseMecanumDrive方法,內容為一般控制方式,即搖桿往前車子以車頭為前方前進,如下圖所示。
(2)在DriveBaseCommand物件裡讀取搖桿數值並呼叫上方SetBaseMecanumDrive方法,如下圖所示。
(3)在RobotContainer裡新增DriverBaseCommand,如下圖所示。
(4)在RobotContainer裡新增SendableChooser,他將可以存放多個Command,如下圖所示。
(5)在RobotContainer 建構子內加入DriveFODCommand(上帝視角運動模式命令)、DriveBaseCommand(基礎運動模式),將Table 命名為「Command」並且將此SendableChooser 加入Shuffleboard,他將會以下拉選單方式呈現。
(5)新增一個GetCommand 方法,他將回傳目前Shuffleboard 所選擇的Command,如下圖所示。
(6)在Robot 物件裡面的TeleopInit 方法內呼叫RobotContainer 內GerCommand 方法取得所選命令並啟動,如下圖所示。
(7)啟動後可在ShuffleBoard Command Tabe 內看到此下拉選單,即包含Default 的基礎運動模式以及上帝視角運動模式,透過下拉選單的方式即可快速切換運動模式,如下圖所示。
七、小結
這次的文章中我們介紹了Shuffleboard 的應用方式,了解如何運用程式新增Shuffleboard UI 元件,了解如何將資料發送到Shuffleboard內,並且在VMX 內透過NetworkTable 讀取Shuffleboard元件狀態,最後將兩種麥克納姆輪運動模式設計成命令,使用Shuffleboard 提供選擇,讓使用者能隨時更換運動方法。喜歡我們文章的夥伴們,請多多支持與分享我們的文章喔!