Added ramp subsystem and minor fixes to algae pivot

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

@@ -18,9 +18,9 @@ AlgaePivotSubsystem::AlgaePivotSubsystem()
   REGISTER_SOFTLIMIT_CONFIG("algae_pivot/algae_pivot_softlimits", true, 1.0);
 
   // bool using_limits =
-  // GetPreferenceValue_bool("elevator/elevator_softlimits/using_limits");
+  // GetPreferenceValue_bool("algae_pivot/algae_pivot_softlimits/using_limits");
   // double reduce_max_dc =
-  // GetPreferenceValue_double("elevator/elevator_softlimits/reduce_max_dc");
+  // GetPreferenceValue_double("algae_pivot/algae_pivot_softlimits/reduce_max_dc");
 
   motor_helper_.SetConversion(algae_pivot_reduction_);
 
@@ -47,7 +47,7 @@ AlgaePivotTarget AlgaePivotSubsystem::ZeroTarget() const {
 bool AlgaePivotSubsystem::VerifyHardware() {
   bool ok = true;
   FRC846_VERIFY(
-      pivot_.VerifyConnected(), ok, "Could not verify elevator motor");
+      pivot_.VerifyConnected(), ok, "Could not verify algae pivot motor");
   return ok;
 }
 

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

@@ -0,0 +1,49 @@
+#include "subsystems/hardware/ramp.h"
+
+#include "subsystems/SubsystemHelper.h"
+
+RampSubsystem::RampSubsystem()
+    : frc846::robot::GenericSubsystem<RampReadings, RampTarget>("ramp"),
+      motor_configs(GET_MOTOR_CONFIG("ramp/ramp_", ports::ramp_::kRamp_CANID,
+          frc846::wpilib::unit_ohm{0.0}, frc846::wpilib::unit_kg_m_sq{0.0})),
+      ramp_(frc846::control::base::MotorMonkeyType::SPARK_MAX_VORTEX,
+          motor_configs) {
+  REGISTER_MOTOR_CONFIG("ramp/ramp_", false, true, 40_A, 40_A, 16.0_V);
+  REGISTER_PIDF_CONFIG("ramp/ramp_gains_", 0.0, 0.0, 0.0, 0.0);
+
+  motor_helper_.SetConversion(ramp_reduction_);
+  motor_helper_.bind(&ramp_);
+}
+
+void RampSubsystem::Setup() {
+  ramp_.Setup();
+  ramp_.EnableStatusFrames(
+      {frc846::control::config::StatusFrame::kVelocityFrame});
+}
+
+RampTarget RampSubsystem::ZeroTarget() const {
+  return RampTarget{RampState::kIdle, 0.0_fps};
+}
+
+bool RampSubsystem::VerifyHardware() {
+  bool ok = true;
+  FRC846_VERIFY(ramp_.VerifyConnected(), ok, "Could not verify ramp motor");
+  return ok;
+}
+
+RampReadings RampSubsystem::ReadFromHardware() {
+  RampReadings readings;
+  readings.vel = motor_helper_.GetVelocity();
+  Graph("readings/ramp_velocity", readings.vel);
+  return readings;
+}
+
+void RampSubsystem::WriteToHardware(RampTarget target) {
+  ramp_.SetGains(GET_PIDF_GAINS("ramp/ramp_gains_"));
+  if (target.state == RampState::kIntake) {
+    target.vel = 2.0_fps;
+  } else {
+    target.vel = 0.0_fps;
+  }
+  motor_helper_.WriteVelocity(target.vel);
+}

src/y2025/include/ports.h

@@ -38,4 +38,8 @@ struct ports {
   struct algae_pivot_ {
     static constexpr int kAlgaePivotOne_CANID = 18;
   };
+
+  struct ramp_ {
+    static constexpr int kRamp_CANID = 20;
+  };
 };

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

@@ -0,0 +1,57 @@
+#pragma once
+
+#include <units/math.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/robot/GenericSubsystem.h"
+#include "frc846/robot/calculators/VerticalArmCalculator.h"
+#include "frc846/wpilib/units.h"
+#include "ports.h"
+
+enum RampState { kIntake, kIdle };
+
+struct RampReadings {
+  RampState state;
+  units::feet_per_second_t vel;
+};
+
+struct RampTarget {
+  RampState state;
+  units::feet_per_second_t vel;
+};
+
+using ramp_pos_conv_t = units::unit_t<
+    units::compound_unit<units::feet, units::inverse<units::turn>>>;
+
+class RampSubsystem
+    : public frc846::robot::GenericSubsystem<RampReadings, RampTarget> {
+public:
+  RampSubsystem();
+
+  void Setup() override;
+
+  RampTarget ZeroTarget() const override;
+
+  bool VerifyHardware() override;
+
+private:
+  bool hasZeroed = false;
+
+  // TODO: Set to correct reduction later
+  ramp_pos_conv_t ramp_reduction_ = 1.0_ft / 1.0_tr;
+
+  frc846::control::config::MotorConstructionParameters motor_configs;
+  frc846::control::HigherMotorController ramp_;
+  frc846::control::HMCHelper<units::feet> motor_helper_;
+
+  frc846::robot::calculators::VerticalArmCalculator arm_calculator_;
+
+  RampReadings ReadFromHardware() override;
+
+  void WriteToHardware(RampTarget target) override;
+};