mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
a865f48e96
In Java, PubSubOption is still used for passing options, but this simplifies C++ use substantially, as it allows aggregate construction.
78 lines
2.9 KiB
C++
78 lines
2.9 KiB
C++
// Copyright (c) FIRST and other WPILib contributors.
|
|
// Open Source Software; you can modify and/or share it under the terms of
|
|
// the WPILib BSD license file in the root directory of this project.
|
|
|
|
#include "glass/networktables/NTFMS.h"
|
|
|
|
#include <stdint.h>
|
|
|
|
#include <fmt/format.h>
|
|
#include <wpi/SmallVector.h>
|
|
#include <wpi/timestamp.h>
|
|
|
|
using namespace glass;
|
|
|
|
NTFMSModel::NTFMSModel(std::string_view path)
|
|
: NTFMSModel{nt::NetworkTableInstance::GetDefault(), path} {}
|
|
|
|
NTFMSModel::NTFMSModel(nt::NetworkTableInstance inst, std::string_view path)
|
|
: m_inst{inst},
|
|
m_gameSpecificMessage{
|
|
inst.GetStringTopic(fmt::format("{}/GameSpecificMessage", path))
|
|
.Subscribe("")},
|
|
m_alliance{inst.GetBooleanTopic(fmt::format("{}/IsRedAlliance", path))
|
|
.Subscribe(false)},
|
|
m_station{inst.GetIntegerTopic(fmt::format("{}/StationNumber", path))
|
|
.Subscribe(0)},
|
|
m_controlWord{inst.GetIntegerTopic(fmt::format("{}/FMSControlData", path))
|
|
.Subscribe(0)},
|
|
m_fmsAttached{fmt::format("NT_FMS:FMSAttached:{}", path)},
|
|
m_dsAttached{fmt::format("NT_FMS:DSAttached:{}", path)},
|
|
m_allianceStationId{fmt::format("NT_FMS:AllianceStationID:{}", path)},
|
|
m_estop{fmt::format("NT_FMS:EStop:{}", path)},
|
|
m_enabled{fmt::format("NT_FMS:RobotEnabled:{}", path)},
|
|
m_test{fmt::format("NT_FMS:TestMode:{}", path)},
|
|
m_autonomous{fmt::format("NT_FMS:AutonomousMode:{}", path)} {
|
|
m_fmsAttached.SetDigital(true);
|
|
m_dsAttached.SetDigital(true);
|
|
m_estop.SetDigital(true);
|
|
m_enabled.SetDigital(true);
|
|
m_test.SetDigital(true);
|
|
m_autonomous.SetDigital(true);
|
|
}
|
|
|
|
std::string_view NTFMSModel::GetGameSpecificMessage(
|
|
wpi::SmallVectorImpl<char>& buf) {
|
|
return m_gameSpecificMessage.Get(buf, "");
|
|
}
|
|
|
|
void NTFMSModel::Update() {
|
|
for (auto&& v : m_alliance.ReadQueue()) {
|
|
int allianceStationId = m_allianceStationId.GetValue();
|
|
allianceStationId %= 3;
|
|
// true if red
|
|
allianceStationId += 3 * (v.value ? 0 : 1);
|
|
m_allianceStationId.SetValue(allianceStationId, v.time);
|
|
}
|
|
for (auto&& v : m_station.ReadQueue()) {
|
|
int allianceStationId = m_allianceStationId.GetValue();
|
|
bool isRed = (allianceStationId < 3);
|
|
// the NT value is 1-indexed
|
|
m_allianceStationId.SetValue(v.value - 1 + 3 * (isRed ? 0 : 1), v.time);
|
|
}
|
|
for (auto&& v : m_controlWord.ReadQueue()) {
|
|
uint32_t controlWord = v.value;
|
|
// See HAL_ControlWord definition
|
|
m_enabled.SetValue(((controlWord & 0x01) != 0) ? 1 : 0, v.time);
|
|
m_autonomous.SetValue(((controlWord & 0x02) != 0) ? 1 : 0, v.time);
|
|
m_test.SetValue(((controlWord & 0x04) != 0) ? 1 : 0, v.time);
|
|
m_estop.SetValue(((controlWord & 0x08) != 0) ? 1 : 0, v.time);
|
|
m_fmsAttached.SetValue(((controlWord & 0x10) != 0) ? 1 : 0, v.time);
|
|
m_dsAttached.SetValue(((controlWord & 0x20) != 0) ? 1 : 0, v.time);
|
|
}
|
|
}
|
|
|
|
bool NTFMSModel::Exists() {
|
|
return m_controlWord.Exists();
|
|
}
|