From 8d1dee16be6fd249182fd94ad93a010ceaae4d8c Mon Sep 17 00:00:00 2001 From: Jaci R Date: Sat, 30 Dec 2017 14:55:31 +1100 Subject: [PATCH] Add DriverStation NetworkTables HAL Extension (#829) --- settings.gradle | 1 + simulation/halsim_ds_nt/build.gradle | 58 ++++++ simulation/halsim_ds_nt/publish.gradle | 94 +++++++++ .../src/main/native/cpp/HALSimDsNt.cpp | 194 ++++++++++++++++++ .../halsim_ds_nt/src/main/native/cpp/main.cpp | 26 +++ .../src/main/native/include/HALSimDsNt.h | 37 ++++ 6 files changed, 410 insertions(+) create mode 100644 simulation/halsim_ds_nt/build.gradle create mode 100644 simulation/halsim_ds_nt/publish.gradle create mode 100644 simulation/halsim_ds_nt/src/main/native/cpp/HALSimDsNt.cpp create mode 100644 simulation/halsim_ds_nt/src/main/native/cpp/main.cpp create mode 100644 simulation/halsim_ds_nt/src/main/native/include/HALSimDsNt.h diff --git a/settings.gradle b/settings.gradle index 853e1904fe..a787eb7446 100644 --- a/settings.gradle +++ b/settings.gradle @@ -11,3 +11,4 @@ include 'myRobot' include 'simulation:halsim_print' include 'simulation:halsim_lowfi' include 'simulation:adx_gyro_accelerometer' +include 'simulation:halsim_ds_nt' diff --git a/simulation/halsim_ds_nt/build.gradle b/simulation/halsim_ds_nt/build.gradle new file mode 100644 index 0000000000..6f9d030e11 --- /dev/null +++ b/simulation/halsim_ds_nt/build.gradle @@ -0,0 +1,58 @@ +description = "A simulation shared object that uses NetworkTables to act as a stand-in for the FRC Driver Station" + +apply plugin: 'edu.wpi.first.NativeUtils' +apply plugin: 'visual-studio' +apply plugin: 'cpp' + +if (!project.hasProperty('onlyAthena')) { + ext.skipAthena = true + + apply from: "../../config.gradle" + + + model { + dependencyConfigs { + ntcore(DependencyConfig) { + groupId = 'edu.wpi.first.ntcore' + artifactId = 'ntcore-cpp' + headerClassifier = 'headers' + ext = 'zip' + version = '4.+' + sharedConfigs = [ halsim_ds_nt: [] ] + } + wpiutil(DependencyConfig) { + groupId = 'edu.wpi.first.wpiutil' + artifactId = 'wpiutil-cpp' + headerClassifier = 'headers' + ext = 'zip' + version = '3.+' + sharedConfigs = [ halsim_ds_nt: [] ] + } + } + components { + halsim_ds_nt(NativeLibrarySpec) { + sources { + cpp { + source { + srcDirs = [ 'src/main/native/cpp' ] + includes = ["**/*.cpp"] + } + exportedHeaders { + srcDirs = ["src/main/native/include"] + } + } + } + } + } + + binaries { + all { + project(':hal').addHalToLinker(it) + } + withType(StaticLibraryBinarySpec) { + it.buildable = false + } + } + } + apply from: 'publish.gradle' +} diff --git a/simulation/halsim_ds_nt/publish.gradle b/simulation/halsim_ds_nt/publish.gradle new file mode 100644 index 0000000000..19ed91316f --- /dev/null +++ b/simulation/halsim_ds_nt/publish.gradle @@ -0,0 +1,94 @@ +apply plugin: 'maven-publish' +apply plugin: 'edu.wpi.first.wpilib.versioning.WPILibVersioningPlugin' + +if (!hasProperty('releaseType')) { + WPILibVersion { + releaseType = 'dev' + } +} + +def pubVersion = '' +if (project.hasProperty("publishVersion")) { + pubVersion = project.publishVersion +} else { + pubVersion = WPILibVersion.version +} + +def baseArtifactId = 'halsim-ds-nt' +def artifactGroupId = 'edu.wpi.first.halsim.ds' + +def outputsFolder = file("$project.buildDir/outputs") + +task cppSourcesZip(type: Zip) { + destinationDir = outputsFolder + baseName = 'halsim-ds-nt' + classifier = "sources" + + from(licenseFile) { + into '/' + } + + from('src/main/native/cpp') { + into '/' + } +} + +task cppHeadersZip(type: Zip) { + destinationDir = outputsFolder + baseName = 'halsim-ds-nt' + classifier = "headers" + + from(licenseFile) { + into '/' + } + + from('src/main/native/include') { + into '/' + } +} + +build.dependsOn cppSourcesZip +build.dependsOn cppHeadersZip + + +model { + publishing { + def pluginTaskList = createComponentZipTasks($.components, 'halsim_ds_nt', 'zipcpp', Zip, project, { task, value-> + value.each { binary-> + if (binary.buildable) { + if (binary instanceof SharedLibraryBinarySpec) { + task.dependsOn binary.buildTask + task.from (binary.sharedLibraryFile) { + into getPlatformPath(binary) + '/shared' + } + } + } + } + }) + + def allTask + if (!project.hasProperty('jenkinsBuild')) { + allTask = createAllCombined(pluginTaskList, 'halsim_ds_nt', 'zipcpp', Zip, project) + } + + publications { + cpp(MavenPublication) { + pluginTaskList.each { + artifact it + } + + if (!project.hasProperty('jenkinsBuild')) { + artifact allTask + } + + artifact cppHeadersZip + artifact cppSourcesZip + + + artifactId = baseArtifactId + groupId artifactGroupId + version pubVersion + } + } + } +} diff --git a/simulation/halsim_ds_nt/src/main/native/cpp/HALSimDsNt.cpp b/simulation/halsim_ds_nt/src/main/native/cpp/HALSimDsNt.cpp new file mode 100644 index 0000000000..71c8d6994c --- /dev/null +++ b/simulation/halsim_ds_nt/src/main/native/cpp/HALSimDsNt.cpp @@ -0,0 +1,194 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "HALSimDsNt.h" + +void HALSimDSNT::Initialize() { + rootTable = + nt::NetworkTableInstance::GetDefault().GetTable("sim")->GetSubTable( + "DS_CONTROL"); // Not to be confused with sim::DriverStation from + // HALSim LowFi + + // LOOP TIMING // + + auto timinghz = rootTable->GetEntry("timing_hz"); + timinghz.ForceSetDouble(50); + timinghz.AddListener( + [this](const nt::EntryNotification& ev) -> void { + double valIn = ev.value->GetDouble(); + double val = 0; + val = (valIn < 1 ? 1 : valIn > 100 ? 100 : valIn); + + if (val != valIn) { + this->rootTable->GetEntry("timing_hz").ForceSetDouble(val); + Flush(); + } + + this->timingHz = val; + }, + NT_NotifyKind::NT_NOTIFY_UPDATE); + + // MODES // + + modeTable = rootTable->GetSubTable("mode"); + auto mtele = modeTable->GetEntry("teleop?"); + auto mauto = modeTable->GetEntry("auto?"); + auto mtest = modeTable->GetEntry("test?"); + auto enabled = modeTable->GetEntry("enabled?"); + auto estop = modeTable->GetEntry("estop?"); + + mtele.ForceSetBoolean(true); + mauto.ForceSetBoolean(false); + mtest.ForceSetBoolean(false); + enabled.ForceSetBoolean(false); + estop.ForceSetBoolean(false); + + mtele.AddListener( + [this](const nt::EntryNotification& ev) -> void { + this->HandleModePress(HALSimDSNT_Mode::teleop, ev.value->GetBoolean()); + }, + NT_NotifyKind::NT_NOTIFY_UPDATE); + + mauto.AddListener( + [this](const nt::EntryNotification& ev) -> void { + this->HandleModePress(HALSimDSNT_Mode::auton, ev.value->GetBoolean()); + }, + NT_NotifyKind::NT_NOTIFY_UPDATE); + + mtest.AddListener( + [this](const nt::EntryNotification& ev) -> void { + this->HandleModePress(HALSimDSNT_Mode::test, ev.value->GetBoolean()); + }, + NT_NotifyKind::NT_NOTIFY_UPDATE); + + enabled.AddListener( + [this](const nt::EntryNotification& ev) -> void { + std::lock_guard lock(modeMutex); + if (!this->isEstop) { + this->isEnabled = ev.value->GetBoolean(); + } else { + this->isEnabled = false; + } + this->DoModeUpdate(); + this->UpdateModeButtons(); + }, + NT_NotifyKind::NT_NOTIFY_UPDATE); + + estop.AddListener( + [this](const nt::EntryNotification& ev) -> void { + std::lock_guard lock(modeMutex); + this->isEstop = ev.value->GetBoolean(); + if (this->isEstop) { + this->isEnabled = false; + } + this->DoModeUpdate(); + this->UpdateModeButtons(); + }, + NT_NotifyKind::NT_NOTIFY_UPDATE); + + // ALLIANCES // + + allianceTable = rootTable->GetSubTable("alliance"); + auto allianceStation = allianceTable->GetEntry("station"); + auto allianceColorRed = allianceTable->GetEntry("red?"); + + allianceStation.ForceSetDouble(1); + allianceColorRed.ForceSetBoolean(true); + + allianceStation.AddListener( + [this](const nt::EntryNotification& ev) -> void { + double stnIn = ev.value->GetDouble(); + double stn = 0; + stn = (stnIn > 3 ? 3 : stnIn < 1 ? 1 : stnIn); + + if (stn != stnIn) { + this->allianceTable->GetEntry("station").ForceSetDouble(stn); + Flush(); + } + + this->allianceStation = stn; + this->DoAllianceUpdate(); + }, + NT_NotifyKind::NT_NOTIFY_UPDATE); + + allianceColorRed.AddListener( + [this](const nt::EntryNotification& ev) -> void { + this->isAllianceRed = ev.value->GetBoolean(); + this->DoAllianceUpdate(); + }, + NT_NotifyKind::NT_NOTIFY_UPDATE); + + // FINAL LOGIC // + + Flush(); + + loopThread = std::thread([this]() -> void { + this->running = true; + this->LoopFunc(); + }); + loopThread.detach(); +} + +void HALSimDSNT::HandleModePress(enum HALSimDSNT_Mode mode, bool isPressed) { + if (isPressed) { + if (mode != currentMode) { + std::lock_guard lock(modeMutex); + currentMode = mode; + isEnabled = false; + this->DoModeUpdate(); + } + } + + this->UpdateModeButtons(); +} + +void HALSimDSNT::UpdateModeButtons() { + modeTable->GetEntry("teleop?").ForceSetBoolean(currentMode == + HALSimDSNT_Mode::teleop); + modeTable->GetEntry("auto?").ForceSetBoolean(currentMode == + HALSimDSNT_Mode::auton); + modeTable->GetEntry("test?").ForceSetBoolean(currentMode == + HALSimDSNT_Mode::test); + modeTable->GetEntry("enabled?").ForceSetBoolean(isEnabled); + Flush(); +} + +void HALSimDSNT::DoModeUpdate() { + HALSIM_SetDriverStationAutonomous(currentMode == HALSimDSNT_Mode::auton); + HALSIM_SetDriverStationTest(currentMode == HALSimDSNT_Mode::test); + HALSIM_SetDriverStationEnabled(isEnabled); + if (isEnabled && !lastIsEnabled) { + currentMatchTime = 0; + } + lastIsEnabled = isEnabled; + HALSIM_SetDriverStationEStop(isEstop); + HALSIM_SetDriverStationFmsAttached(false); + HALSIM_SetDriverStationDsAttached(true); + HALSIM_NotifyDriverStationNewData(); +} + +void HALSimDSNT::DoAllianceUpdate() { + HALSIM_SetDriverStationAllianceStationId(static_cast( + (isAllianceRed ? HAL_AllianceStationID_kRed1 + : HAL_AllianceStationID_kBlue1) + + (static_cast(allianceStation) - 1))); +} + +void HALSimDSNT::LoopFunc() { + while (running) { + double dt = 1000 / timingHz; + std::this_thread::sleep_for( + std::chrono::milliseconds(static_cast(dt))); + if (isEnabled) { + currentMatchTime = currentMatchTime + dt; + HALSIM_SetDriverStationMatchTime(currentMatchTime); + } + HALSIM_NotifyDriverStationNewData(); + } +} + +void HALSimDSNT::Flush() { rootTable->GetInstance().Flush(); } diff --git a/simulation/halsim_ds_nt/src/main/native/cpp/main.cpp b/simulation/halsim_ds_nt/src/main/native/cpp/main.cpp new file mode 100644 index 0000000000..2a64c41c3b --- /dev/null +++ b/simulation/halsim_ds_nt/src/main/native/cpp/main.cpp @@ -0,0 +1,26 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include + +static HALSimDSNT dsnt; + +extern "C" { +#if defined(WIN32) || defined(_WIN32) +__declspec(dllexport) +#endif + int HALSIM_InitExtension(void) { + std::cout << "DriverStationNT Initializing." << std::endl; + + dsnt.Initialize(); + + std::cout << "DriverStationNT Initialized!" << std::endl; + return 0; +} +} // extern "C" diff --git a/simulation/halsim_ds_nt/src/main/native/include/HALSimDsNt.h b/simulation/halsim_ds_nt/src/main/native/include/HALSimDsNt.h new file mode 100644 index 0000000000..d67624be6e --- /dev/null +++ b/simulation/halsim_ds_nt/src/main/native/include/HALSimDsNt.h @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include + +#include +#include +#include + +enum HALSimDSNT_Mode { teleop, auton, test }; + +class HALSimDSNT { + public: + std::shared_ptr rootTable, modeTable, allianceTable; + enum HALSimDSNT_Mode currentMode; + bool isEnabled, lastIsEnabled, isEstop; + std::atomic isAllianceRed, running; + std::atomic currentMatchTime, timingHz, allianceStation; + std::thread loopThread; + wpi::mutex modeMutex; + + void Initialize(); + void HandleModePress(enum HALSimDSNT_Mode mode, bool isPressed); + void UpdateModeButtons(); + void DoModeUpdate(); + void DoAllianceUpdate(); + void LoopFunc(); + void Flush(); +};