From b68fd9ec3014d965cabbd715cfeffe1b605d1354 Mon Sep 17 00:00:00 2001
From: prak132 <rathoreprakhar1@gmail.com>
Date: Wed, 15 Jan 2025 16:39:23 -0800
Subject: [PATCH] Added vertical arm control to algae pivot and fixed issues
 with vertical arm calculator

---
 .../robot/calculators/VerticalArmCalculator.cc      | 13 +++++--------
 .../robot/calculators/VerticalArmCalculator.h       |  9 +--------
 src/y2025/cpp/subsystems/hardware/algae_pivot.cc    |  6 +++++-
 src/y2025/include/subsystems/SubsystemHelper.h      |  8 ++++----
 src/y2025/include/subsystems/hardware/algae_pivot.h |  3 +++
 5 files changed, 18 insertions(+), 21 deletions(-)

diff --git a/src/frc846/cpp/frc846/robot/calculators/VerticalArmCalculator.cc b/src/frc846/cpp/frc846/robot/calculators/VerticalArmCalculator.cc
index 52f9c6d..75c1c7a 100644
--- a/src/frc846/cpp/frc846/robot/calculators/VerticalArmCalculator.cc
+++ b/src/frc846/cpp/frc846/robot/calculators/VerticalArmCalculator.cc
@@ -6,16 +6,13 @@
 
 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;
@@ -23,8 +20,8 @@ double VerticalArmCalculator::calculate(VerticalArmInputs inputs) {
   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::clamp(duty_cycle, constants_.peak_output_reverse,
+      constants_.peak_output_forward);
 }
 
 }  // namespace frc846::robot::calculators
\ No newline at end of file
diff --git a/src/frc846/include/frc846/robot/calculators/VerticalArmCalculator.h b/src/frc846/include/frc846/robot/calculators/VerticalArmCalculator.h
index 38fab9b..bc1587c 100644
--- a/src/frc846/include/frc846/robot/calculators/VerticalArmCalculator.h
+++ b/src/frc846/include/frc846/robot/calculators/VerticalArmCalculator.h
@@ -20,8 +20,6 @@ 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]
 */
@@ -29,8 +27,6 @@ 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
diff --git a/src/y2025/cpp/subsystems/hardware/algae_pivot.cc b/src/y2025/cpp/subsystems/hardware/algae_pivot.cc
index dbd00a1..a7f65b3 100644
--- a/src/y2025/cpp/subsystems/hardware/algae_pivot.cc
+++ b/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_")}));
 }
\ No newline at end of file
diff --git a/src/y2025/include/subsystems/SubsystemHelper.h b/src/y2025/include/subsystems/SubsystemHelper.h
index a89c3ae..4121cd7 100644
--- a/src/y2025/include/subsystems/SubsystemHelper.h
+++ b/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"),    \
diff --git a/src/y2025/include/subsystems/hardware/algae_pivot.h b/src/y2025/include/subsystems/hardware/algae_pivot.h
index 028b31b..4ad667b 100644
--- a/src/y2025/include/subsystems/hardware/algae_pivot.h
+++ b/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;