mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
b60eb1544b
A few virtual functions are called by constructors or destructors, which is dangerous in C++ (as an overridden virtual impl won't be called, only the one in the current class). Fix by either marking the function final or not calling at all (if possible).
90 lines
3.2 KiB
C++
90 lines
3.2 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 <wpi/Twine.h>
|
|
#include <wpi/timestamp.h>
|
|
|
|
using namespace glass;
|
|
|
|
NTFMSModel::NTFMSModel(wpi::StringRef path)
|
|
: NTFMSModel{nt::GetDefaultInstance(), path} {}
|
|
|
|
NTFMSModel::NTFMSModel(NT_Inst inst, wpi::StringRef path)
|
|
: m_nt{inst},
|
|
m_gameSpecificMessage{m_nt.GetEntry(path + "/GameSpecificMessage")},
|
|
m_alliance{m_nt.GetEntry(path + "/IsRedAlliance")},
|
|
m_station{m_nt.GetEntry(path + "/StationNumber")},
|
|
m_controlWord{m_nt.GetEntry(path + "/FMSControlData")},
|
|
m_fmsAttached{"NT_FMS:FMSAttached:" + path},
|
|
m_dsAttached{"NT_FMS:DSAttached:" + path},
|
|
m_allianceStationId{"NT_FMS:AllianceStationID:" + path},
|
|
m_estop{"NT_FMS:EStop:" + path},
|
|
m_enabled{"NT_FMS:RobotEnabled:" + path},
|
|
m_test{"NT_FMS:TestMode:" + path},
|
|
m_autonomous{"NT_FMS:AutonomousMode:" + path} {
|
|
m_nt.AddListener(m_alliance);
|
|
m_nt.AddListener(m_station);
|
|
m_nt.AddListener(m_controlWord);
|
|
|
|
m_fmsAttached.SetDigital(true);
|
|
m_dsAttached.SetDigital(true);
|
|
m_estop.SetDigital(true);
|
|
m_enabled.SetDigital(true);
|
|
m_test.SetDigital(true);
|
|
m_autonomous.SetDigital(true);
|
|
}
|
|
|
|
wpi::StringRef NTFMSModel::GetGameSpecificMessage(
|
|
wpi::SmallVectorImpl<char>& buf) {
|
|
buf.clear();
|
|
auto value = nt::GetEntryValue(m_gameSpecificMessage);
|
|
if (value && value->IsString()) {
|
|
auto str = value->GetString();
|
|
buf.append(str.begin(), str.end());
|
|
}
|
|
return wpi::StringRef{buf.data(), buf.size()};
|
|
}
|
|
|
|
void NTFMSModel::Update() {
|
|
for (auto&& event : m_nt.PollListener()) {
|
|
if (event.entry == m_alliance) {
|
|
if (event.value && event.value->IsBoolean()) {
|
|
int allianceStationId = m_allianceStationId.GetValue();
|
|
allianceStationId %= 3;
|
|
// true if red
|
|
allianceStationId += 3 * (event.value->GetBoolean() ? 0 : 1);
|
|
m_allianceStationId.SetValue(allianceStationId);
|
|
}
|
|
} else if (event.entry == m_station) {
|
|
if (event.value && event.value->IsDouble()) {
|
|
int allianceStationId = m_allianceStationId.GetValue();
|
|
bool isRed = (allianceStationId < 3);
|
|
// the NT value is 1-indexed
|
|
m_allianceStationId.SetValue(event.value->GetDouble() - 1 +
|
|
3 * (isRed ? 0 : 1));
|
|
}
|
|
} else if (event.entry == m_controlWord) {
|
|
if (event.value && event.value->IsDouble()) {
|
|
uint32_t controlWord = event.value->GetDouble();
|
|
// See HAL_ControlWord definition
|
|
auto time = wpi::Now();
|
|
m_enabled.SetValue(((controlWord & 0x01) != 0) ? 1 : 0, time);
|
|
m_autonomous.SetValue(((controlWord & 0x02) != 0) ? 1 : 0, time);
|
|
m_test.SetValue(((controlWord & 0x04) != 0) ? 1 : 0, time);
|
|
m_estop.SetValue(((controlWord & 0x08) != 0) ? 1 : 0, time);
|
|
m_fmsAttached.SetValue(((controlWord & 0x10) != 0) ? 1 : 0, time);
|
|
m_dsAttached.SetValue(((controlWord & 0x20) != 0) ? 1 : 0, time);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
bool NTFMSModel::Exists() {
|
|
return m_nt.IsConnected() && nt::GetEntryType(m_controlWord) != NT_UNASSIGNED;
|
|
}
|