[sim] Update sim match time to match real robot (#4024)

The real robot has match time set to -1.0 until it's enabled, and then
counts down. Disabling the robot sets the time to -1.0.

The sim GUI has been updated to add preset buttons for auto and teleop
match times. The enable match timing checkbox has been removed as it's
no longer required.

The DS socket plugin has also been fixed to properly initialize
matchTime to -1.0 and reset it to -1.0 on disable.

glass/src/lib/native/cpp/other/FMS.cpp

@@ -14,7 +14,7 @@ using namespace glass;
 static const char* stations[] = {"Red 1",  "Red 2",  "Red 3",
                                  "Blue 1", "Blue 2", "Blue 3"};
 
-void glass::DisplayFMS(FMSModel* model, bool* matchTimeEnabled) {
+void glass::DisplayFMS(FMSModel* model) {
   if (!model->Exists() || model->IsReadOnly()) {
     return DisplayFMSReadOnly(model);
   }
@@ -49,10 +49,6 @@ void glass::DisplayFMS(FMSModel* model, bool* matchTimeEnabled) {
 
   // Match Time
   if (auto data = model->GetMatchTimeData()) {
-    if (matchTimeEnabled) {
-      ImGui::Checkbox("Match Time Enabled", matchTimeEnabled);
-    }
-
     double val = data->GetValue();
     ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8);
     if (ImGui::InputDouble("Match Time", &val, 0, 0, "%.1f",
@@ -60,9 +56,17 @@ void glass::DisplayFMS(FMSModel* model, bool* matchTimeEnabled) {
       model->SetMatchTime(val);
     }
     data->EmitDrag();
+    bool enabled = false;
+    if (auto enabledData = model->GetEnabledData()) {
+      enabled = enabledData->GetValue();
+    }
     ImGui::SameLine();
-    if (ImGui::Button("Reset")) {
-      model->SetMatchTime(0.0);
+    if (ImGui::Button("Auto") && !enabled) {
+      model->SetMatchTime(15.0);
+    }
+    ImGui::SameLine();
+    if (ImGui::Button("Teleop") && !enabled) {
+      model->SetMatchTime(135.0);
     }
   }
 

glass/src/lib/native/include/glass/other/FMS.h

@@ -47,7 +47,7 @@ class FMSModel : public Model {
  * @param matchTimeEnabled If not null, a checkbox is displayed for
  *                         "enable match time" linked to this value
  */
-void DisplayFMS(FMSModel* model, bool* matchTimeEnabled = nullptr);
+void DisplayFMS(FMSModel* model);
 void DisplayFMSReadOnly(FMSModel* model);
 
 }  // namespace glass

hal/src/main/native/sim/mockdata/DriverStationData.cpp

@@ -30,7 +30,7 @@ void DriverStationData::ResetData() {
   fmsAttached.Reset(false);
   dsAttached.Reset(true);
   allianceStationId.Reset(static_cast<HAL_AllianceStationID>(0));
-  matchTime.Reset(0.0);
+  matchTime.Reset(-1.0);
 
   {
     std::scoped_lock lock(m_joystickDataMutex);

hal/src/main/native/sim/mockdata/DriverStationDataInternal.h

@@ -126,7 +126,7 @@ class DriverStationData {
   SimDataValue<HAL_AllianceStationID, MakeAllianceStationIdValue,
                GetAllianceStationIdName>
       allianceStationId{static_cast<HAL_AllianceStationID>(0)};
-  SimDataValue<double, HAL_MakeDouble, GetMatchTimeName> matchTime{0.0};
+  SimDataValue<double, HAL_MakeDouble, GetMatchTimeName> matchTime{-1.0};
 
  private:
   SimCallbackRegistry<HAL_JoystickAxesCallback, GetJoystickAxesName>

simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp

@@ -310,6 +310,10 @@ void DSCommPacket::SetupJoystickTag(wpi::raw_uv_ostream& buf) {
 void DSCommPacket::SendUDPToHALSim(void) {
   SendJoysticks();
 
+  if (!m_control_word.enabled) {
+    m_match_time = -1;
+  }
+
   HALSIM_SetDriverStationMatchTime(m_match_time);
   HALSIM_SetDriverStationEnabled(m_control_word.enabled);
   HALSIM_SetDriverStationAutonomous(m_control_word.autonomous);

simulation/halsim_ds_socket/src/main/native/include/DSCommPacket.h

@@ -66,7 +66,7 @@ class DSCommPacket {
   HAL_AllianceStationID m_alliance_station;
   HAL_MatchInfo matchInfo;
   std::array<DSCommJoystickPacket, HAL_kMaxJoysticks> m_joystick_packets;
-  double m_match_time;
+  double m_match_time = -1;
 };
 
 }  // namespace halsim

simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp

@@ -247,8 +247,6 @@ class FMSSimModel : public glass::FMSModel {
   }
   void SetMatchTime(double val) override {
     HALSIM_SetDriverStationMatchTime(val);
-    int32_t status = 0;
-    m_startMatchTime = HAL_GetFPGATime(&status) * 1.0e-6 - val;
   }
   void SetEStop(bool val) override { HALSIM_SetDriverStationEStop(val); }
   void SetEnabled(bool val) override { HALSIM_SetDriverStationEnabled(val); }
@@ -266,8 +264,6 @@ class FMSSimModel : public glass::FMSModel {
 
   bool IsReadOnly() override;
 
-  bool m_matchTimeEnabled = true;
-
  private:
   glass::DataSource m_fmsAttached{"FMS:FMSAttached"};
   glass::DataSource m_dsAttached{"FMS:DSAttached"};
@@ -277,8 +273,7 @@ class FMSSimModel : public glass::FMSModel {
   glass::DataSource m_enabled{"FMS:RobotEnabled"};
   glass::DataSource m_test{"FMS:TestMode"};
   glass::DataSource m_autonomous{"FMS:AutonomousMode"};
-  double m_startMatchTime = 0.0;
-  double m_prevTime = 0.0;
+  double m_startMatchTime = -1.0;
 };
 
 }  // namespace
@@ -1138,6 +1133,7 @@ FMSSimModel::FMSSimModel() {
   m_enabled.SetDigital(true);
   m_test.SetDigital(true);
   m_autonomous.SetDigital(true);
+  m_matchTime.SetValue(-1.0);
 }
 
 void FMSSimModel::Update() {
@@ -1151,25 +1147,23 @@ void FMSSimModel::Update() {
   m_autonomous.SetValue(HALSIM_GetDriverStationAutonomous());
 
   double matchTime = HALSIM_GetDriverStationMatchTime();
-  if (m_matchTimeEnabled && !IsDSDisabled()) {
+  if (!IsDSDisabled() && enabled) {
     int32_t status = 0;
     double curTime = HAL_GetFPGATime(&status) * 1.0e-6;
-    if (m_startMatchTime == 0.0) {
-      m_startMatchTime = curTime;
+    if (m_startMatchTime == -1.0) {
+      m_startMatchTime = matchTime + curTime;
     }
-    if (enabled) {
-      matchTime = curTime - m_startMatchTime;
-      HALSIM_SetDriverStationMatchTime(matchTime);
-    } else {
-      if (m_prevTime == 0.0) {
-        m_prevTime = curTime;
-      }
-      m_startMatchTime += (curTime - m_prevTime);
+    matchTime = m_startMatchTime - curTime;
+    if (matchTime < 0) {
+      matchTime = -1.0;
     }
-    m_prevTime = curTime;
+    HALSIM_SetDriverStationMatchTime(matchTime);
   } else {
-    m_startMatchTime = 0.0;
-    m_prevTime = 0.0;
+    if (m_startMatchTime != -1.0) {
+      matchTime = -1.0;
+      HALSIM_SetDriverStationMatchTime(matchTime);
+    }
+    m_startMatchTime = -1.0;
   }
   m_matchTime.SetValue(matchTime);
 }
@@ -1424,9 +1418,8 @@ void DriverStationGui::GlobalInit() {
         win->SetDefaultSize(300, 560);
       }
     }
-    if (auto win = dsManager->AddWindow("FMS", [] {
-          DisplayFMS(gFMSModel.get(), &gFMSModel->m_matchTimeEnabled);
-        })) {
+    if (auto win =
+            dsManager->AddWindow("FMS", [] { DisplayFMS(gFMSModel.get()); })) {
       win->DisableRenamePopup();
       win->SetFlags(ImGuiWindowFlags_AlwaysAutoResize);
       win->SetDefaultPos(5, 540);