Elevator subsystem and subsystem helper

README.md

@@ -173,8 +173,9 @@ To undo the going back:
 
 ## CppCheck Warnings
 ```
-src\frc846\cpp\frc846\robot\swerve\drivetrain.cc:160:64: warning: Variable 'accel_target' is assigned a value that is never used. [unreadVariable]
-src\frc846\cpp\frc846\math\collection.cc:25:0: warning: The function 'VerticalDeadband' is never used. [unusedFunction]
-src\frc846\cpp\frc846\math\collection.cc:39:0: warning: The function 'CoterminalDifference' is never used. [unusedFunction]
-src\frc846\cpp\frc846\math\collection.cc:52:0: warning: The function 'CoterminalSum' is never used. [unusedFunction]
+src/frc846/cpp/frc846/control/calculators/CurrentTorqueCalculator.cc:49:30: warning: Variable 'duty_cycle_original' is assigned a value that is never used. [unreadVariable]
+src/frc846/cpp/frc846/robot/swerve/drivetrain.cc:160:64: warning: Variable 'accel_target' is assigned a value that is never used. [unreadVariable]
+src/frc846/cpp/frc846/math/collection.cc:25:0: warning: The function 'VerticalDeadband' is never used. [unusedFunction]
+src/frc846/cpp/frc846/math/collection.cc:39:0: warning: The function 'CoterminalDifference' is never used. [unusedFunction]
+src/frc846/cpp/frc846/math/collection.cc:52:0: warning: The function 'CoterminalSum' is never used. [unusedFunction]
 ```

src/y2025/cpp/subsystems/hardware/elevator.cc

@@ -0,0 +1,62 @@
+#include "subsystems/hardware/elevator.h"
+
+#include "subsystems/SubsystemHelper.h"
+
+ElevatorSubsystem::ElevatorSubsystem()
+    : frc846::robot::GenericSubsystem<ElevatorReadings, ElevatorTarget>(
+          "elevator"),
+      motor_configs(GET_MOTOR_CONFIG("elevator/elevator_one_",
+          ports::elevator_::kElevatorOne_CANID, frc846::wpilib::unit_ohm{0.0},
+          frc846::wpilib::unit_kg_m_sq{0.0})),
+      elevator_(frc846::control::base::MotorMonkeyType::TALON_FX_KRAKENX60,
+          motor_configs) {
+  RegisterPreference("elevator/elevator_tolerance_", 0.25_in);
+
+  REGISTER_MOTOR_CONFIG(
+      "elevator/elevator_one_", false, true, 40_A, 40_A, 16.0_V);
+  REGISTER_PIDF_CONFIG("elevator/elevator_gains_", 0.0, 0.0, 0.0, 0.0);
+  REGISTER_SOFTLIMIT_CONFIG("elevator/elevator_softlimits", true, 1.0);
+
+  // bool using_limits =
+  //     GetPreferenceValue_bool("elevator/elevator_softlimits/using_limits");
+  // double reduce_max_dc =
+  //     GetPreferenceValue_double("elevator/elevator_softlimits/reduce_max_dc");
+
+  motor_helper_.SetConversion(elevator_reduction_);
+
+  // motor_helper_.SetSoftLimits(
+  //     using_limits, 96.0_in, 0.0_in, 90.0_in, 6.0_in, reduce_max_dc);
+
+  motor_helper_.bind(&elevator_);
+}
+
+void ElevatorSubsystem::Setup() {
+  elevator_.Setup();
+  elevator_.EnableStatusFrames(
+      {frc846::control::config::StatusFrame::kPositionFrame,
+          frc846::control::config::StatusFrame::kVelocityFrame});
+  motor_helper_.SetPosition(0.0_in);
+}
+
+ElevatorTarget ElevatorSubsystem::ZeroTarget() const {
+  return ElevatorTarget{0.0_in};
+}
+
+bool ElevatorSubsystem::VerifyHardware() {
+  bool ok = true;
+  FRC846_VERIFY(
+      elevator_.VerifyConnected(), ok, "Could not verify elevator motor");
+  return ok;
+}
+
+ElevatorReadings ElevatorSubsystem::ReadFromHardware() {
+  ElevatorReadings readings;
+  readings.height = motor_helper_.GetPosition();
+  Graph("readings/elevator_pos", readings.height);
+  return readings;
+}
+
+void ElevatorSubsystem::WriteToHardware(ElevatorTarget target) {
+  elevator_.SetGains(GET_PIDF_GAINS("elevator/elevator_gains_"));
+  motor_helper_.WritePosition(target.height);
+}

src/y2025/include/ports.h

@@ -30,4 +30,8 @@ struct ports {
   struct leds_ {  // TODO: Confirm LED ports
     static constexpr int kLEDStrip1 = 6;
   };
+
+  struct elevator_ {
+    static constexpr int kElevatorOne_CANID = 15;
+  };
 };

src/y2025/include/subsystems/SubsystemHelper.h

@@ -0,0 +1,45 @@
+#pragma once
+
+#include <units/current.h>
+#include <units/voltage.h>
+
+#include "frc846/control/base/motor_gains.h"
+#include "frc846/control/config/construction_params.h"
+
+#define REGISTER_MOTOR_CONFIG(subsystem_path, inverted, brake_mode,    \
+    current_limit, smart_current_limit, voltage_compensation)          \
+  RegisterPreference(#subsystem_path "/inverted", inverted);           \
+  RegisterPreference(#subsystem_path "/brake_mode", brake_mode);       \
+  RegisterPreference(#subsystem_path "/current_limit", current_limit); \
+  RegisterPreference(                                                  \
+      #subsystem_path "/smart_current_limit", smart_current_limit);    \
+  RegisterPreference(                                                  \
+      #subsystem_path "/voltage_compensation", voltage_compensation)
+
+#define GET_MOTOR_CONFIG(                                           \
+    subsystem_path, can_id, circuit_resistance, rotational_inertia) \
+  {can_id, GetPreferenceValue_bool(#subsystem_path "/inverted"),    \
+      GetPreferenceValue_bool(#subsystem_path "/brake_mode"),       \
+      GetPreferenceValue_unit_type<units::ampere_t>(                \
+          #subsystem_path "/current_limit"),                        \
+      GetPreferenceValue_unit_type<units::ampere_t>(                \
+          #subsystem_path "/smart_current_limit"),                  \
+      GetPreferenceValue_unit_type<units::volt_t>(                  \
+          #subsystem_path "/voltage_compensation"),                 \
+      circuit_resistance, rotational_inertia}
+
+#define REGISTER_PIDF_CONFIG(subsystem_path, p, i, d, f) \
+  RegisterPreference(#subsystem_path "/kP", p);          \
+  RegisterPreference(#subsystem_path "/kI", i);          \
+  RegisterPreference(#subsystem_path "/kD", d);          \
+  RegisterPreference(#subsystem_path "/kF", f)
+
+#define GET_PIDF_GAINS(subsystem_path)                   \
+  {GetPreferenceValue_double(#subsystem_path "/_kP"),    \
+      GetPreferenceValue_double(#subsystem_path "/_kI"), \
+      GetPreferenceValue_double(#subsystem_path "/_kD"), \
+      GetPreferenceValue_double(#subsystem_path "/_kF")}
+
+#define REGISTER_SOFTLIMIT_CONFIG(subsystem_path, use_limits, reduce_max_dc) \
+  RegisterPreference(#subsystem_path "/using_limits", use_limits);           \
+  RegisterPreference(#subsystem_path "/reduce_max_dc", reduce_max_dc)

src/y2025/include/subsystems/hardware/elevator.h

@@ -0,0 +1,59 @@
+#pragma once
+
+#include <units/angle.h>
+#include <units/constants.h>
+#include <units/length.h>
+#include <units/math.h>
+#include <units/torque.h>
+#include <units/velocity.h>
+
+#include "frc846/base/Loggable.h"
+#include "frc846/control/HMCHelper.h"
+#include "frc846/control/HigherMotorController.h"
+#include "frc846/control/base/motor_control_base.h"
+#include "frc846/control/config/construction_params.h"
+#include "frc846/control/config/soft_limits.h"
+#include "frc846/robot/GenericSubsystem.h"
+#include "frc846/wpilib/units.h"
+#include "ports.h"
+
+struct ElevatorReadings {
+  units::inch_t height;
+};
+
+struct ElevatorTarget {
+  units::inch_t height;
+};
+
+using elevator_pos_conv_t = units::unit_t<
+    units::compound_unit<units::inch, units::inverse<units::turn>>>;
+
+class ElevatorSubsystem
+    : public frc846::robot::GenericSubsystem<ElevatorReadings, ElevatorTarget> {
+public:
+  ElevatorSubsystem();
+
+  void Setup() override;
+
+  ElevatorTarget ZeroTarget() const override;
+
+  bool VerifyHardware() override;
+
+  bool WithinTolerance(units::inch_t pos) {
+    return units::math::abs(pos - GetReadings().height) <
+           GetPreferenceValue_unit_type<units::inch_t>("elevator_tolerance_");
+  }
+
+private:
+  bool hasZeroed = false;
+
+  elevator_pos_conv_t elevator_reduction_ = 1.0_in / 1.0_tr;
+
+  frc846::control::config::MotorConstructionParameters motor_configs;
+  frc846::control::HigherMotorController elevator_;
+  frc846::control::HMCHelper<units::inch> motor_helper_;
+
+  ElevatorReadings ReadFromHardware() override;
+
+  void WriteToHardware(ElevatorTarget target) override;
+};