Merge pull request #32 from Team846/elevator_subsystem

merging the subsystem code

README.md

@@ -175,8 +175,6 @@ To undo the going back:
 ```
 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/y2025/cpp/subsystems/hardware/climber.cc:50:3: warning: Consecutive return, break, continue, goto or throw statements are unnecessary. [duplicateBreak]
-src/y2025/cpp/subsystems/hardware/coral_wrist.cc:40:3: warning: Consecutive return, break, continue, goto or throw statements are unnecessary. [duplicateBreak]
 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/robot/calculators/VerticalArmCalculator.cc

@@ -1,30 +1,25 @@
 #include "frc846/robot/calculators/VerticalArmCalculator.h"
 
-#include <units/angle.h>
-#include <units/angular_velocity.h>
 #include <units/math.h>
 
 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::clamp(duty_cycle, constants_.peak_output_reverse,
+      constants_.peak_output_forward);
 }
 
 }  // namespace frc846::robot::calculators

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

@@ -4,11 +4,11 @@
 #include <units/angular_velocity.h>
 #include <units/length.h>
 #include <units/mass.h>
+#include <units/torque.h>
 
 #include "frc846/control/base/motor_gains.h"
 #include "frc846/math/calculator.h"
 #include "frc846/math/constants.h"
-#include "frc846/wpilib/units.h"
 
 namespace frc846::robot::calculators {
 
@@ -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

@@ -0,0 +1,66 @@
+#include "subsystems/hardware/algae_pivot.h"
+
+#include "subsystems/SubsystemHelper.h"
+
+AlgaePivotSubsystem::AlgaePivotSubsystem()
+    : frc846::robot::GenericSubsystem<AlgaePivotReadings, AlgaePivotTarget>(
+          "algae_pivot"),
+      motor_configs(GET_MOTOR_CONFIG("algae_pivot/algae_pivot_one_",
+          ports::algae_pivot_::kAlgaePivotOne_CANID,
+          frc846::wpilib::unit_ohm{0.0}, frc846::wpilib::unit_kg_m_sq{0.0})),
+      pivot_(frc846::control::base::MotorMonkeyType::SPARK_MAX_VORTEX,
+          motor_configs) {
+  RegisterPreference("algae_pivot_tolerance_", 0.25_deg);
+
+  REGISTER_MOTOR_CONFIG(
+      "algae_pivot/algae_pivot_one_", false, true, 40_A, 40_A, 16.0_V);
+  REGISTER_PIDF_CONFIG("algae_pivot/algae_pivot_gains_", 0.0, 0.0, 0.0, 0.0);
+  REGISTER_SOFTLIMIT_CONFIG("algae_pivot/algae_pivot_softlimits", true, 1.0);
+
+  // bool using_limits =
+  // GetPreferenceValue_bool("algae_pivot/algae_pivot_softlimits/using_limits");
+  // double reduce_max_dc =
+  // GetPreferenceValue_double("algae_pivot/algae_pivot_softlimits/reduce_max_dc");
+
+  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);
+
+  motor_helper_.bind(&pivot_);
+}
+
+void AlgaePivotSubsystem::Setup() {
+  pivot_.Setup();
+  pivot_.EnableStatusFrames(
+      {frc846::control::config::StatusFrame::kPositionFrame,
+          frc846::control::config::StatusFrame::kVelocityFrame});
+  motor_helper_.SetPosition(0.0_deg);
+}
+
+AlgaePivotTarget AlgaePivotSubsystem::ZeroTarget() const {
+  return AlgaePivotTarget{0.0_deg};
+}
+
+bool AlgaePivotSubsystem::VerifyHardware() {
+  bool ok = true;
+  FRC846_VERIFY(
+      pivot_.VerifyConnected(), ok, "Could not verify algae pivot motor");
+  return ok;
+}
+
+AlgaePivotReadings AlgaePivotSubsystem::ReadFromHardware() {
+  AlgaePivotReadings readings;
+  readings.position = motor_helper_.GetPosition();
+  Graph("readings/algae_pivot_position", readings.position);
+  return readings;
+}
+
+void AlgaePivotSubsystem::WriteToHardware(AlgaePivotTarget target) {
+  pivot_.SetGains(GET_PIDF_GAINS("algae_pivot/algae_pivot_gains_"));
+  motor_helper_.WriteDC(arm_calculator_.calculate({motor_helper_.GetPosition(),
+      target.position, motor_helper_.GetVelocity(),
+      GET_PIDF_GAINS("algae_pivot/algae_pivot_gains_")}));
+}

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

@@ -10,7 +10,7 @@ ElevatorSubsystem::ElevatorSubsystem()
           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);
+  RegisterPreference("elevator_tolerance_", 0.25_in);
 
   REGISTER_MOTOR_CONFIG(
       "elevator/elevator_one_", false, true, 40_A, 40_A, 16.0_V);
@@ -52,7 +52,7 @@ bool ElevatorSubsystem::VerifyHardware() {
 ElevatorReadings ElevatorSubsystem::ReadFromHardware() {
   ElevatorReadings readings;
   readings.height = motor_helper_.GetPosition();
-  Graph("readings/elevator_pos", readings.height);
+  Graph("readings/elevator_position", readings.height);
   return readings;
 }