diff --git a/README.md b/README.md index d6040b6..e8153cd 100644 --- a/README.md +++ b/README.md @@ -168,4 +168,20 @@ src/frc846/cpp/frc846/other/trajectory_generator.cc:68:18: warning: Consider usi src/y2024/cpp/commands/teleop/drive_command.cc:67:8: warning: Condition 'is_robot_centric' is always false [knownConditionTrueFalse] src/frc846/cpp/frc846/util/math.cc:19:0: warning: The function 'VerticalDeadband' is never used. [unusedFunction] src/frc846/cpp/frc846/util/math.cc:46:0: warning: The function 'CoterminalSum' is never used. [unusedFunction] +``` +## CppCheck Warnings +``` +srcfrc846cppfrc846ntinffstore.cc:287:27: warning: Consider using std::any_of algorithm instead of a raw loop. [useStlAlgorithm] +srcy2024cppcommandsteleopdrive_command.cc:69:8: warning: Condition 'is_robot_centric' is always false [knownConditionTrueFalse] +srcy2024cppcommandsteleopdrive_command.cc:33:17: warning: Variable 'intaking' is assigned a value that is never used. [unreadVariable] +srcy2024cppfield.cc:9:16: warning: Variable 'point' can be declared as reference to const [constVariableReference] +srcy2024cppfield.cc:21:14: warning: Variable 'path' can be declared as reference to const [constVariableReference] +srcy2024cppfield.cc:10:32: warning: Consider using std::find_if algorithm instead of a raw loop. [useStlAlgorithm] +srcy2024cppfield.cc:22:29: warning: Consider using std::find_if algorithm instead of a raw loop. [useStlAlgorithm] +srcy2024cppfield.cc:67:10: warning: Consider using std::replace_if algorithm instead of a raw loop. [useStlAlgorithm] +srcy2024cppfield.cc:77:10: warning: Consider using std::replace_if algorithm instead of a raw loop. [useStlAlgorithm] +srcy2024cppfield.cc:60:10: warning: Consider using std::replace_if algorithm instead of a raw loop. [useStlAlgorithm] +srcfrc846cppfrc846mathcollection.cc:7:0: warning: The function 'DEquals' is never used. [unusedFunction] +srcfrc846cppfrc846mathcollection.cc:25:0: warning: The function 'VerticalDeadband' is never used. [unusedFunction] +srcfrc846cppfrc846mathcollection.cc:52:0: warning: The function 'CoterminalSum' is never used. [unusedFunction] ``` \ No newline at end of file diff --git a/build.gradle b/build.gradle index 592b2b5..07148e5 100644 --- a/build.gradle +++ b/build.gradle @@ -33,7 +33,7 @@ deploy { def deployArtifact = deploy.targets.roborio.artifacts.frcCpp // Set this to true to enable desktop support. -def includeDesktopSupport = true +def includeDesktopSupport = false // Set to true to run simulation in debug mode wpi.cpp.debugSimulation = false @@ -82,27 +82,27 @@ model { } } - testSuites { - frcUserProgramTest(GoogleTestTestSuiteSpec) { - testing $.components.frcUserProgram - - println "Configuring frcUserProgramTest..." - sources.cpp { - source { - srcDir 'src/test/cpp' - include '**/*.cpp' - } - } - binaries.all { - cppCompiler.args << '-DNOMINMAX' - println "Applied compiler argument NOMINMAX to allow for compilation on Windows platform." - } - wpi.cpp.enableExternalTasks(it) - wpi.cpp.vendor.cpp(it) - wpi.cpp.deps.wpilib(it) - wpi.cpp.deps.googleTest(it) - } - } + // testSuites { + // frcUserProgramTest(GoogleTestTestSuiteSpec) { + // testing $.components.frcUserProgram + + // println "Configuring frcUserProgramTest..." + // sources.cpp { + // source { + // srcDir 'src/test/cpp' + // include '**/*.cpp' + // } + // } + // binaries.all { + // cppCompiler.args << '-DNOMINMAX' + // println "Applied compiler argument NOMINMAX to allow for compilation on Windows platform." + // } + // wpi.cpp.enableExternalTasks(it) + // wpi.cpp.vendor.cpp(it) + // wpi.cpp.deps.wpilib(it) + // wpi.cpp.deps.googleTest(it) + // } + // } } spotless { @@ -111,7 +111,7 @@ spotless { include '**/*.cpp', '**/*.cc', '**/*.h', '**/*.hpp' exclude '**/build/**', '**/build-*/**' } - def selectedClangVersion = project.hasProperty('fromCI') ? '14.0.0-1ubuntu1.1' : '19.1.1' + def selectedClangVersion = project.hasProperty('fromCI') ? '14.0.0-1ubuntu1.1' : '18.1.8' clangFormat(selectedClangVersion).style('Google') } groovyGradle { diff --git a/src/deploy/autos/scripts/amp_side_nothing b/src/deploy/autos/scripts/amp_side_nothing new file mode 100644 index 0000000..9292a86 --- /dev/null +++ b/src/deploy/autos/scripts/amp_side_nothing @@ -0,0 +1,4 @@ +0,2 +F,kRightSub +ACT,prep_shoot +ACT,shoot \ No newline at end of file diff --git a/src/deploy/autos/scripts/four_piece_auto b/src/deploy/autos/scripts/four_piece_auto index 99421ec..cd081c7 100644 --- a/src/deploy/autos/scripts/four_piece_auto +++ b/src/deploy/autos/scripts/four_piece_auto @@ -1,5 +1,6 @@ 0,2 F,kPointBlank +ACT,auto_home ACT,prep_shoot ACT,shoot ACT,deploy_intake diff --git a/src/deploy/autos/scripts/source_side_auto b/src/deploy/autos/scripts/source_side_auto index 0819bd5..5faab59 100644 --- a/src/deploy/autos/scripts/source_side_auto +++ b/src/deploy/autos/scripts/source_side_auto @@ -1,5 +1,6 @@ 0,2 F,kLeftSub +ACT,auto_home ACT,prep_shoot ACT,shoot ACT,deploy_intake diff --git a/src/deploy/autos/scripts/source_side_nothing b/src/deploy/autos/scripts/source_side_nothing new file mode 100644 index 0000000..f27a3f2 --- /dev/null +++ b/src/deploy/autos/scripts/source_side_nothing @@ -0,0 +1,4 @@ +0,2 +F,kLeftSub +ACT,prep_shoot +ACT,shoot \ No newline at end of file diff --git a/src/y2024/cpp/commands/basic/trap_command.cc b/src/y2024/cpp/commands/basic/trap_command.cc index e91d71a..6dc520d 100644 --- a/src/y2024/cpp/commands/basic/trap_command.cc +++ b/src/y2024/cpp/commands/basic/trap_command.cc @@ -4,55 +4,35 @@ TrapCommand::TrapCommand(RobotContainer& container, int stage) : frc846::robot::GenericCommand{container, "trap_command"}, stage_{stage} { - AddRequirements({&container_.intake_, &container_.shooter_, - &container_.super_structure_}); + AddRequirements({&container_.super_structure_}); } void TrapCommand::OnInit() {} void TrapCommand::Periodic() { if (stage_ == 1) { - container_.intake_.SetTarget(container_.intake_.ZeroTarget()); - container_.shooter_.SetTarget(container_.shooter_.ZeroTarget()); - container_.super_structure_.SetTargetSetpoint( container_.super_structure_.getPreClimbSetpoint()); } if (stage_ == 2) { - container_.intake_.SetTarget(container_.intake_.ZeroTarget()); - container_.shooter_.SetTarget(container_.shooter_.ZeroTarget()); - auto pullClimbTarget = container_.super_structure_.getPreClimbSetpoint(); pullClimbTarget.pivot = container_.super_structure_.pivot_pull_target_.value(); container_.super_structure_.SetTargetSetpoint(pullClimbTarget, true); } if (stage_ == 3) { - container_.intake_.SetTarget(container_.intake_.ZeroTarget()); - container_.shooter_.SetTarget(container_.shooter_.ZeroTarget()); - container_.super_structure_.SetTargetSetpoint( container_.super_structure_.getPreScoreSetpoint()); } if (stage_ == 4) { - container_.intake_.SetTarget(container_.intake_.ZeroTarget()); - container_.shooter_.SetTarget(container_.shooter_.ZeroTarget()); - container_.super_structure_.SetTargetSetpoint( container_.super_structure_.getTrapScoreSetpoint()); } if (stage_ == 5) { - container_.shooter_.SetTarget(container_.shooter_.ZeroTarget()); - - container_.intake_.SetTarget({IntakeState::kRelease}); - container_.super_structure_.SetTargetSetpoint( container_.super_structure_.getTrapScoreSetpoint()); } if (stage_ == 6) { - container_.intake_.SetTarget(container_.intake_.ZeroTarget()); - container_.shooter_.SetTarget(container_.shooter_.ZeroTarget()); - container_.super_structure_.SetTargetSetpoint( container_.super_structure_.getPostScoreSetpoint()); } diff --git a/src/y2024/cpp/commands/complex/home_during_auto_command.cc b/src/y2024/cpp/commands/complex/home_during_auto_command.cc index f25ca0f..280953f 100644 --- a/src/y2024/cpp/commands/complex/home_during_auto_command.cc +++ b/src/y2024/cpp/commands/complex/home_during_auto_command.cc @@ -11,18 +11,20 @@ #include "frc2/command/WaitUntilCommand.h" HomeDuringAutoCommand::HomeDuringAutoCommand(RobotContainer& container) - : frc846::robot::GenericCommandGroup{ - container, "home_during_auto_command", - frc2::SequentialCommandGroup{frc2::ParallelDeadlineGroup{ - frc2::WaitUntilCommand{[&] { - return container_.super_structure_.wrist_->GetHasZeroed(); - }}, - frc2::SequentialCommandGroup{ - frc2::ParallelDeadlineGroup{ - frc2::WaitUntilCommand{[&] { - return container.pivot_.WithinTolerance( - container.super_structure_.getStowSetpoint().pivot); - }}, - StowCommand{container}}, - WristZeroCommand{container}}}}} {} \ No newline at end of file + : frc846::robot::GenericCommandGroup< + RobotContainer, HomeDuringAutoCommand, + frc2::SequentialCommandGroup>{container, "home_during_auto_command", + frc2::SequentialCommandGroup{ + frc2::ParallelDeadlineGroup{ + frc2::WaitUntilCommand{[&] { + return container.pivot_ + .WithinTolerance( + container + .super_structure_ + .getStowSetpoint() + .pivot); + }}, + StowCommand{container}}, + WristZeroCommand{container}}} + +{} \ No newline at end of file diff --git a/src/y2024/cpp/commands/teleop/bracer_command.cc b/src/y2024/cpp/commands/teleop/bracer_command.cc index 494f747..1b3c8bc 100644 --- a/src/y2024/cpp/commands/teleop/bracer_command.cc +++ b/src/y2024/cpp/commands/teleop/bracer_command.cc @@ -11,13 +11,13 @@ void BracerCommand::OnInit() {} void BracerCommand::Periodic() { ControlInputReadings ci_readings_{container_.control_input_.GetReadings()}; - BracerTarget target = container_.bracer_.ZeroTarget(); + BracerTarget target = {kStow}; - if (ci_readings_.stageOfTrap != 0) { - target.state = BracerState::kExtend; - } else { - target.state = BracerState::kRetract; - } + // if (ci_readings_.stageOfTrap != 0) { + // target.state = BracerState::kExtend; + // } else { + // target.state = BracerState::kRetract; + // } ci_readings_.stageOfTrap = std::max(ci_readings_.stageOfTrap, 0); @@ -31,9 +31,9 @@ void BracerCommand::Periodic() { else target.state = BracerState::kStow; - if (container_.control_input_.operator_.GetReadings().y_button) { + if (container_.control_input_.operator_.GetReadings().right_trigger) { target.state = BracerState::kRetract; - } else if (container_.control_input_.operator_.GetReadings().b_button) { + } else if (container_.control_input_.operator_.GetReadings().right_bumper) { target.state = BracerState::kExtend; } diff --git a/src/y2024/cpp/subsystems/abstract/gpd.cc b/src/y2024/cpp/subsystems/abstract/gpd.cc new file mode 100644 index 0000000..2b22516 --- /dev/null +++ b/src/y2024/cpp/subsystems/abstract/gpd.cc @@ -0,0 +1,91 @@ +#include "subsystems/abstract/gpd.h" + +#include +#include + +#include + +#include "field.h" +#include "frc846/ntinf/pref.h" +#include "frc846/util/share_tables.h" + +GPDSubsystem::GPDSubsystem(bool init) + : frc846::robot::GenericSubsystem("gpd", init) { + if (init) { +#ifndef _WIN32 + for (int i = 0; i < 20; i++) { + g_field.GetObject(std::to_string(i)); + } + frc::SmartDashboard::PutData("NoteField", &g_field); +#endif + } +} + +GPDTarget GPDSubsystem::ZeroTarget() const { + GPDTarget target; + return target; +} + +bool GPDSubsystem::VerifyHardware() { return true; } + +frc846::math::Vector2D GPDSubsystem::findDistance(units::degree_t theta_h, + units::degree_t theta_v) { + units::foot_t height = mount_height_.value() - note_height_.value(); + auto dist = units::math::abs(height * units::math::tan(theta_v)); + auto yDist = dist * units::math::cos(theta_h); + auto xDist = dist * units::math::sin(theta_h); + + return {xDist, yDist}; +} + +GPDReadings GPDSubsystem::ReadFromHardware() { + GPDReadings readings{}; + + units::degree_t bearing_ = + units::degree_t(frc846::util::ShareTables::GetDouble("robot_bearing_")); + + units::inch_t robot_x = + units::foot_t(frc846::util::ShareTables::GetDouble("odometry_x_")); + + units::inch_t robot_y = + units::foot_t(frc846::util::ShareTables::GetDouble("odometry_y_")); + + units::feet_per_second_t velocity_x = units::feet_per_second_t( + frc846::util::ShareTables::GetDouble("velocity_x_")); + + units::feet_per_second_t velocity_y = units::feet_per_second_t( + frc846::util::ShareTables::GetDouble("velocity_y_")); + + std::vector theta_hs = gpdTable->GetNumberArray("theta_h", {}); + std::vector theta_vs = gpdTable->GetNumberArray("theta_v", {}); + auto latency = gpdTable->GetNumber("latency", 0.1) * 1_s; + + for (size_t i = 0; i < theta_hs.size(); i++) { + auto distance = findDistance(units::degree_t(theta_hs[i]), + units::degree_t(theta_vs[i])); + auto pos = + distance.rotate(bearing_) + frc846::math::Vector2D{robot_x, robot_y} + + frc846::math::Vector2D{velocity_x * latency, velocity_y * latency}; + + readings.notes.push_back(pos); + } + + int num_notes = readings.notes.size(); + Graph("notes_detected", num_notes); + +#ifndef _WIN32 + for (int i = 0; i < std::min(20, num_notes); i++) { + auto pos = readings.notes[i]; + g_field.GetObject(std::to_string(i)) + ->SetPose(pos[0], pos[1], frc::Rotation2d(0_deg)); + } + for (int i = std::min(20, num_notes); i < 20; i++) { + g_field.GetObject(std::to_string(i)) + ->SetPose(5000_ft, 5000_ft, frc::Rotation2d(0_deg)); + } +#endif + + return readings; +} + +void GPDSubsystem::WriteToHardware(GPDTarget target) {} \ No newline at end of file diff --git a/src/y2024/cpp/subsystems/hardware/drivetrain.cc b/src/y2024/cpp/subsystems/hardware/drivetrain.cc index b846886..c1621cb 100644 --- a/src/y2024/cpp/subsystems/hardware/drivetrain.cc +++ b/src/y2024/cpp/subsystems/hardware/drivetrain.cc @@ -6,10 +6,6 @@ #include "frc846/wpilib/time.h" #include "subsystems/hardware/swerve_module.h" -units::feet_per_second_t vel_readings_composite; -double vel_readings_composite_x; -double vel_readings_composite_y; - DrivetrainSubsystem::DrivetrainSubsystem(bool initialize) : frc846::robot::GenericSubsystem{"drivetrain", diff --git a/src/y2024/include/subsystems/abstract/gpd.h b/src/y2024/include/subsystems/abstract/gpd.h new file mode 100644 index 0000000..45e5437 --- /dev/null +++ b/src/y2024/include/subsystems/abstract/gpd.h @@ -0,0 +1,59 @@ +#pragma once + +#include +#include +#include +#include + +#include "frc/smartdashboard/Smartdashboard.h" +#include "frc846/math/vectors.h" +#include "frc846/ntinf/grapher.h" +#include "frc846/robot/GenericSubsystem.h" +#include "networktables/NetworkTable.h" +#include "networktables/NetworkTableEntry.h" +#include "networktables/NetworkTableInstance.h" +#include "networktables/NetworkTableValue.h" +#include "ports.h" +#include "subsystems/hardware/drivetrain.h" + +struct GPDTarget {}; +struct GPDReadings { + std::vector notes; +}; + +class GPDSubsystem + : public frc846::robot::GenericSubsystem { + public: + GPDSubsystem(bool init); + + void Setup() override {}; + + frc846::math::Vector2D findDistance(units::degree_t theta_h, + units::degree_t theta_v); + + GPDTarget ZeroTarget() const override; + + bool VerifyHardware() override; + + private: + frc::Field2d g_field; + + GPDReadings prevReadings; + + frc846::base::Loggable readings_named{*this, "readings"}; + + frc846::base::Loggable algo_params{*this, "algo_parameters"}; + frc846::ntinf::Pref mount_height_{algo_params, "mount_height", + 1_ft}; + frc846::ntinf::Pref note_height_{algo_params, "note_height", + 0_ft}; + frc846::ntinf::Pref nt_latency{algo_params, "nt_latency", + 0_s}; + + std::shared_ptr gpdTable = + nt::NetworkTableInstance::GetDefault().GetTable("gpd"); + + GPDReadings ReadFromHardware() override; + + void WriteToHardware(GPDTarget target) override; +}; \ No newline at end of file diff --git a/src/y2024/include/subsystems/hardware/drivetrain.h b/src/y2024/include/subsystems/hardware/drivetrain.h index a879584..a94d209 100644 --- a/src/y2024/include/subsystems/hardware/drivetrain.h +++ b/src/y2024/include/subsystems/hardware/drivetrain.h @@ -173,6 +173,10 @@ class DrivetrainSubsystem bool VerifyHardware() override; private: + units::feet_per_second_t vel_readings_composite; + double vel_readings_composite_x; + double vel_readings_composite_y; + // Drivetrain dimensions. frc846::ntinf::Pref width_{*this, "width", 21.75_in}; frc846::ntinf::Pref height_{*this, "height", 26.75_in}; diff --git a/src/y2024/include/subsystems/robot_container.h b/src/y2024/include/subsystems/robot_container.h index 9a4761f..2a199a5 100644 --- a/src/y2024/include/subsystems/robot_container.h +++ b/src/y2024/include/subsystems/robot_container.h @@ -4,6 +4,7 @@ #include "frc846/robot/GenericRobotContainer.h" #include "subsystems/abstract/control_input.h" #include "subsystems/abstract/driver.h" +#include "subsystems/abstract/gpd.h" #include "subsystems/abstract/operator.h" #include "subsystems/abstract/super_structure.h" #include "subsystems/abstract/vision.h" @@ -19,14 +20,15 @@ class RobotContainer : public frc846::robot::GenericRobotContainer { public: frc846::ntinf::Pref init_drivetrain_{*this, "init_drivetrain", true}; - frc846::ntinf::Pref init_intake_{*this, "init_intake", false}; - frc846::ntinf::Pref init_shooter_{*this, "init_shooter", false}; - frc846::ntinf::Pref init_wrist_{*this, "init_wrist", false}; + frc846::ntinf::Pref init_intake_{*this, "init_intake", true}; + frc846::ntinf::Pref init_shooter_{*this, "init_shooter", true}; + frc846::ntinf::Pref init_wrist_{*this, "init_wrist", true}; frc846::ntinf::Pref init_pivot_{*this, "init_pivot", true}; - frc846::ntinf::Pref init_telescope_{*this, "init_telescope", false}; + frc846::ntinf::Pref init_telescope_{*this, "init_telescope", true}; frc846::ntinf::Pref init_leds_{*this, "init_leds", true}; frc846::ntinf::Pref init_bracer_{*this, "init_bracers_", false}; frc846::ntinf::Pref init_vision_{*this, "init_vision_", true}; + frc846::ntinf::Pref init_gpd_{*this, "init_gpd_", false}; ControlInputSubsystem control_input_; DrivetrainSubsystem drivetrain_{init_drivetrain_.value()}; @@ -39,10 +41,11 @@ class RobotContainer : public frc846::robot::GenericRobotContainer { LEDsSubsystem leds_{init_leds_.value()}; SuperStructureSubsystem super_structure_{&pivot_, &wrist_, &telescope_}; VisionSubsystem vision_{init_vision_.value()}; + GPDSubsystem gpd_{init_gpd_.value()}; RobotContainer() { RegisterSubsystems({&control_input_, &drivetrain_, &intake_, &shooter_, &wrist_, &pivot_, &telescope_, &bracer_, &leds_, - &super_structure_, &vision_}); + &super_structure_, &vision_, &gpd_}); } }; diff --git a/src/y2024/resources/deploy_autos_usb.bat b/src/y2024/resources/deploy_autos_usb.bat new file mode 100644 index 0000000..eca6be3 --- /dev/null +++ b/src/y2024/resources/deploy_autos_usb.bat @@ -0,0 +1,3 @@ +scp ./src/deploy/autos/points.lst admin@172.22.11.2:/home/lvuser/deploy/autos +scp ./src/deploy/autos/paths/* admin@172.22.11.2:/home/lvuser/deploy/autos/paths +scp ./src/deploy/autos/scripts/* admin@172.22.11.2:/home/lvuser/deploy/autos/scripts \ No newline at end of file diff --git a/src/y2024/resources/gpd/best.onnx b/src/y2024/resources/gpd/best.onnx new file mode 100644 index 0000000..4dc07df Binary files /dev/null and b/src/y2024/resources/gpd/best.onnx differ diff --git a/src/y2024/resources/gpd/main.py b/src/y2024/resources/gpd/main.py new file mode 100644 index 0000000..dde4ada --- /dev/null +++ b/src/y2024/resources/gpd/main.py @@ -0,0 +1,91 @@ +import cv2 +from ultralytics import YOLO +import numpy as np +import threading +import math + +from networktables import NetworkTables +from pref import NumericPref, BooleanPref, KillSwitch + +import time + + +model = YOLO('best.onnx', task="detect") + +cap = cv2.VideoCapture(0) +cap.set(cv2.CAP_PROP_BUFFERSIZE, 0) +cap.set(cv2.CAP_PROP_FRAME_WIDTH, 256) +cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 256) + + + +queue = [] + +NetworkTables.initialize(server='10.8.46.2') + +gpd_table = NetworkTables.getTable('gpd') +preferenceTable = NetworkTables.getTable("RPIPrefs") + +kill_switch = KillSwitch(preferenceTable) + +gpd_table.putNumberArray('note_x', []) +gpd_table.putNumberArray('note_y', []) + +def readCamera(): + while True: + ret, frame = cap.read() + if ret: + queue.append(frame) + if (len(queue) > 5): + queue.pop(0) + + if cv2.waitKey(1) == ord('q'): + break + + + +def processFrame(): + while True: + start = time.perf_counter() + if len(queue) == 0: + continue + frame = queue.pop(0) + result = model.predict(frame, imgsz=256, conf=0.2, iou=0.5)[0] + + if result is None: continue + + theta_h = [] + theta_v = [] + for box in result.boxes: + bounding_box = box.xyxy[0] + x1, y1, x2, y2 = bounding_box + width = x2 - x1 + height = y2 - y1 + point_width = x1 + width/2 + point_height = y1 + height/2 + theta_h.append((133.8 / 2.0) * (point_width - 0.5*256) / 256) + theta_v.append(-(133.8 / 2.0) * (point_height - 0.5*256) / 256) + + + gpd_table.putNumberArray('theta_h', theta_h) + gpd_table.putNumberArray('theta_v', theta_v) + + + end = time.perf_counter() - start + + gpd_table.putNumber('latency', end) + NetworkTables.flush() + +if __name__ == "__main__": + t1 = threading.Thread(target=readCamera) + t2 = threading.Thread(target=processFrame) + + + t1.start() + t2.start() + + t1.join() + t2.join() + + +cap.release() \ No newline at end of file diff --git a/src/y2024/resources/gpd/pref.py b/src/y2024/resources/gpd/pref.py new file mode 100644 index 0000000..d23b5c2 --- /dev/null +++ b/src/y2024/resources/gpd/pref.py @@ -0,0 +1,137 @@ +from networktables import NetworkTable +from networktables.util import NetworkTablesInstance +import subprocess + +class NumericStore: + def __init__(self): + self.store = {} + self.read() + + def read(self): + print("Reading prefs.") + try: + save_file = open("prefs.numeric", "r") + prefs = save_file.readlines() + for x in prefs: + key, value = x.split(":") + self.store[key] = float(value) + save_file.close() + except: + print("Error reading prefs (numeric).") + + def save(self): + print("Save triggered.") + save_file = open("prefs.numeric", "w") + for key in self.store: + save_file.write(f"{key}:{round(self.store[key], 2)}\n") + save_file.close() + + def put(self, key: str, value: float): + self.store[key] = value + self.save() + + def get(self, key: str) -> float: + if self.store[key] is None: + return 0.0 + return self.store[key] + + def has(self, key: str) -> bool: + return key in self.store + +class BooleanStore: + def __init__(self): + self.store = {} + self.read() + + def read(self): + print("Reading prefs.") + try: + save_file = open("prefs.boolean", "r") + prefs = save_file.readlines() + for x in prefs: + key, value = x.split(":") + self.store[key] = bool(value) + save_file.close() + except: + print("Error reading prefs (boolean).") + + def save(self): + print("Save triggered.") + save_file = open("prefs.boolean", "w") + for key in self.store: + save_file.write(f"{key}:{self.store[key]}\n") + save_file.close() + + def put(self, key: str, value: bool): + self.store[key] = value + self.save() + + def get(self, key: str) -> bool: + if self.store[key] is None: + return False + return self.store[key] + + def has(self, key: str) -> bool: + return key in self.store + +class NumericPref: + store = NumericStore() + + @staticmethod + def valueChanged(table, key, value, isNew): + print(f"Numeric Value changed: {key} -> {value}.") + NumericPref.store.put(key, float(value)) + + def __init__(self, table: NetworkTable, key: str, defaultValue: float): + self.defaultValue = defaultValue + self.key = key + + self.entry = table.getEntry(key) + + if (NumericPref.store.has(key) is False): + NumericPref.store.put(key, defaultValue) + self.entry.forceSetDouble(NumericPref.store.get(key)) + + self.entry.addListener(self.valueChanged, NetworkTablesInstance.NotifyFlags.UPDATE) + + self.entry.setPersistent() + + def get(self) -> float: + return self.entry.getDouble(self.defaultValue) + +class BooleanPref: + store = BooleanStore() + + @staticmethod + def valueChanged(table, key, value, isNew): + print(f"Boolean Value changed: {key} -> {value}.") + BooleanPref.store.put(key, bool(value)) + + def __init__(self, table: NetworkTable, key: str, defaultValue: bool): + self.defaultValue = defaultValue + self.key = key + + self.entry = table.getEntry(key) + + if (BooleanPref.store.has(key) is False): + BooleanPref.store.put(key, defaultValue) + self.entry.forceSetBoolean(BooleanPref.store.get(key)) + + self.entry.addListener(self.valueChanged, NetworkTablesInstance.NotifyFlags.UPDATE) + + self.entry.setPersistent() + + def get(self) -> bool: + return self.entry.getBoolean(self.defaultValue) + +class KillSwitch: + @staticmethod + def reboot(table, key, value, isNew): + if bool(value): + subprocess.run(["reboot"]) + + def __init__(self, table: NetworkTable): + self.entry = table.getEntry("kill") + self.entry.forceSetBoolean(False) + + self.entry.addListener(self.reboot, NetworkTablesInstance.NotifyFlags.UPDATE) \ No newline at end of file