Added vertical arm control to algae pivot and fixed issues with verti…

…cal arm calculator

src/frc846/cpp/frc846/robot/calculators/VerticalArmCalculator.cc

@@ -6,25 +6,22 @@
 
 namespace frc846::robot::calculators {
 
-VerticalArmCalculator::VerticalArmCalculator(VerticalArmConfigs& configs)
-    : configs_(configs) {
-  setConstants(configs);
-}
+VerticalArmCalculator::VerticalArmCalculator() {}
 
 double VerticalArmCalculator::calculate(VerticalArmInputs inputs) {
   units::newton_meter_t gravity_torque =
-      configs_.arm_mass * configs_.center_of_mass *
+      constants_.arm_mass * constants_.center_of_mass *
       frc846::math::constants::physics::g *
-      units::math::cos(inputs.arm_position + configs_.offset_angle);
+      units::math::cos(inputs.arm_position + constants_.offset_angle);
 
   units::degree_t position_error =
       inputs.target_arm_position - inputs.arm_position;
 
   double duty_cycle = inputs.motor_gains.calculate(position_error.to<double>(),
       0.0, inputs.current_velocity.to<double>(), gravity_torque.to<double>());
 
-  return std::max(std::min(duty_cycle, configs_.peak_output_forward),
-      configs_.peak_output_reverse);
+  return std::max(std::min(duty_cycle, constants_.peak_output_forward),
+      constants_.peak_output_reverse);
 }
 
 }  // namespace frc846::robot::calculators

src/frc846/include/frc846/robot/calculators/VerticalArmCalculator.h

@@ -20,17 +20,13 @@ Contains all parameters necessary to construct a new Vertical Arm Calculator.
 @param arm_mass: The mass of the arm.
 @param center_of_mass: The distance from the pivot point to center of mass.
 @param offset_angle: The fixed angle offset relative to the horizontal axis.
-@param max_pos: The maximum allowed position.
-@param min_pos: The minimum allowed position.
 @param peak_output_forward: The maximum duty cycle for forward motion [0, 1]
 @param peak_output_reverse: The maximum duty cycle for reverse motion [-1, 0]
 */
 struct VerticalArmConfigs {
   units::kilogram_t arm_mass;
   units::inch_t center_of_mass;
   units::degree_t offset_angle = 0.0_deg;
-  units::degree_t max_pos;
-  units::degree_t min_pos;
   double peak_output_forward = 1.0;
   double peak_output_reverse = -1.0;
 };
@@ -62,7 +58,7 @@ characteristics of an arm.
 class VerticalArmCalculator : public frc846::math::Calculator<VerticalArmInputs,
                                   double, VerticalArmConfigs> {
 public:
-  VerticalArmCalculator(VerticalArmConfigs& configs);
+  VerticalArmCalculator();
 
   /*
   calculate()
@@ -76,9 +72,6 @@ class VerticalArmCalculator : public frc846::math::Calculator<VerticalArmInputs,
   @return A duty cycle
   */
   double calculate(VerticalArmInputs inputs) override;
-
-private:
-  VerticalArmConfigs& configs_;
 };
 
 }  // namespace frc846::robot::calculators

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

@@ -24,6 +24,8 @@ AlgaePivotSubsystem::AlgaePivotSubsystem()
 
   motor_helper_.SetConversion(algae_pivot_reduction_);
 
+  arm_calculator_.setConstants({20.0_kg, 3.0_in, 0.0_deg, -1.0, 1.0});
+
   // motor_helper_.SetSoftLimits(
   //     using_limits, 90_deg, 0.0_deg, 80_deg, 5_deg, reduce_max_dc);
 
@@ -58,5 +60,7 @@ AlgaePivotReadings AlgaePivotSubsystem::ReadFromHardware() {
 
 void AlgaePivotSubsystem::WriteToHardware(AlgaePivotTarget target) {
   pivot_.SetGains(GET_PIDF_GAINS("algae_pivot/algae_pivot_gains_"));
-  motor_helper_.WritePosition(target.position);
+  motor_helper_.WriteDC(arm_calculator_.calculate({motor_helper_.GetPosition(),
+      target.position, motor_helper_.GetVelocity(),
+      GET_PIDF_GAINS("algae_pivot/algae_pivot_gains_")}));
 }

src/y2025/include/subsystems/SubsystemHelper.h

@@ -28,10 +28,10 @@
       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)
+  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"),    \

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

@@ -8,6 +8,7 @@
 #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"
 
@@ -50,6 +51,8 @@ class AlgaePivotSubsystem
   frc846::control::HigherMotorController pivot_;
   frc846::control::HMCHelper<units::degree> motor_helper_;
 
+  frc846::robot::calculators::VerticalArmCalculator arm_calculator_;
+
   AlgaePivotReadings ReadFromHardware() override;
 
   void WriteToHardware(AlgaePivotTarget target) override;