From 648f00562e83783defcbcdec53b35ca2d558340f Mon Sep 17 00:00:00 2001
From: prak132 <rathoreprakhar1@gmail.com>
Date: Tue, 14 Jan 2025 22:32:30 -0800
Subject: [PATCH] algae pivot subsystem

---
 .../cpp/subsystems/hardware/algae_pivot.cc    | 62 +++++++++++++++++++
 src/y2025/cpp/subsystems/hardware/elevator.cc |  2 +-
 src/y2025/include/ports.h                     |  4 ++
 .../include/subsystems/hardware/algae_pivot.h | 55 ++++++++++++++++
 4 files changed, 122 insertions(+), 1 deletion(-)
 create mode 100644 src/y2025/cpp/subsystems/hardware/algae_pivot.cc
 create mode 100644 src/y2025/include/subsystems/hardware/algae_pivot.h

diff --git a/src/y2025/cpp/subsystems/hardware/algae_pivot.cc b/src/y2025/cpp/subsystems/hardware/algae_pivot.cc
new file mode 100644
index 0000000..74d5f40
--- /dev/null
+++ b/src/y2025/cpp/subsystems/hardware/algae_pivot.cc
@@ -0,0 +1,62 @@
+#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/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("elevator/elevator_softlimits/using_limits");
+  // double reduce_max_dc =
+  // GetPreferenceValue_double("elevator/elevator_softlimits/reduce_max_dc");
+
+  motor_helper_.SetConversion(algae_pivot_reduction_);
+
+  // 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 elevator motor");
+  return ok;
+}
+
+AlgaePivotReadings AlgaePivotSubsystem::ReadFromHardware() {
+  AlgaePivotReadings readings;
+  readings.position = motor_helper_.GetPosition();
+  Graph("algae_pivot/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_.WritePosition(target.position);
+}
\ No newline at end of file
diff --git a/src/y2025/cpp/subsystems/hardware/elevator.cc b/src/y2025/cpp/subsystems/hardware/elevator.cc
index d9195a7..d553957 100644
--- a/src/y2025/cpp/subsystems/hardware/elevator.cc
+++ b/src/y2025/cpp/subsystems/hardware/elevator.cc
@@ -52,7 +52,7 @@ bool ElevatorSubsystem::VerifyHardware() {
 ElevatorReadings ElevatorSubsystem::ReadFromHardware() {
   ElevatorReadings readings;
   readings.height = motor_helper_.GetPosition();
-  Graph("readings/elevator_pos", readings.height);
+  Graph("elevator/readings/elevator_position", readings.height);
   return readings;
 }
 
diff --git a/src/y2025/include/ports.h b/src/y2025/include/ports.h
index 14bc273..738ec0f 100644
--- a/src/y2025/include/ports.h
+++ b/src/y2025/include/ports.h
@@ -34,4 +34,8 @@ struct ports {
   struct elevator_ {
     static constexpr int kElevatorOne_CANID = 15;
   };
+
+  struct algae_pivot_ {
+    static constexpr int kAlgaePivotOne_CANID = 18;
+  };
 };
diff --git a/src/y2025/include/subsystems/hardware/algae_pivot.h b/src/y2025/include/subsystems/hardware/algae_pivot.h
new file mode 100644
index 0000000..4393bf0
--- /dev/null
+++ b/src/y2025/include/subsystems/hardware/algae_pivot.h
@@ -0,0 +1,55 @@
+#pragma once
+
+#include <units/math.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/wpilib/units.h"
+#include "ports.h"
+
+struct AlgaePivotReadings {
+  units::degree_t position;
+};
+
+struct AlgaePivotTarget {
+  units::degree_t position;
+};
+
+using elevator_pos_conv_t = units::unit_t<
+    units::compound_unit<units::degree, units::inverse<units::turn>>>;
+
+class AlgaePivotSubsystem
+    : public frc846::robot::GenericSubsystem<AlgaePivotReadings,
+          AlgaePivotTarget> {
+public:
+  AlgaePivotSubsystem();
+
+  void Setup() override;
+
+  AlgaePivotTarget ZeroTarget() const override;
+
+  bool VerifyHardware() override;
+
+  bool WithinTolerance(units::degree_t pos) {
+    return units::math::abs(pos - GetReadings().position) <
+           GetPreferenceValue_unit_type<units::degree_t>(
+               "algae_pivot_tolerance_");
+  }
+
+private:
+  bool hasZeroed = false;
+
+  elevator_pos_conv_t algae_pivot_reduction_ = 1.0_deg / 1.0_tr;
+
+  frc846::control::config::MotorConstructionParameters motor_configs;
+  frc846::control::HigherMotorController pivot_;
+  frc846::control::HMCHelper<units::degree> motor_helper_;
+
+  AlgaePivotReadings ReadFromHardware() override;
+
+  void WriteToHardware(AlgaePivotTarget target) override;
+};
\ No newline at end of file