Added algae pivot subsystem and updated files paths to remove redunda…

…nt included files
Team846 · Jan 15, 2025 · e2ee334 · e2ee334
1 parent a32314c
commit e2ee334
Show file tree

Hide file tree

Showing 6 changed files with 126 additions and 9 deletions.
diff --git a/src/y2025/cpp/subsystems/hardware/algae_pivot.cc 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);
+}
diff --git a/src/y2025/cpp/subsystems/hardware/elevator.cc b/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;
 }
 

diff --git 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/SubsystemHelper.h b/src/y2025/include/subsystems/SubsystemHelper.h
@@ -3,8 +3,7 @@
 #include <units/current.h>
 #include <units/voltage.h>
 
-#include "frc846/control/base/motor_gains.h"
-#include "frc846/control/config/construction_params.h"
+#include "frc846/base/Loggable.h"
 
 #define REGISTER_MOTOR_CONFIG(subsystem_path, inverted, brake_mode,    \
     current_limit, smart_current_limit, voltage_compensation)          \

diff --git a/src/y2025/include/subsystems/hardware/algae_pivot.h b/src/y2025/include/subsystems/hardware/algae_pivot.h
@@ -0,0 +1,56 @@
+#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;
+
+  // TODO: Set to correct reduction later
+  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;
+};
diff --git a/src/y2025/include/subsystems/hardware/elevator.h b/src/y2025/include/subsystems/hardware/elevator.h
@@ -1,18 +1,13 @@
 #pragma once
 
-#include <units/angle.h>
-#include <units/constants.h>
 #include <units/length.h>
 #include <units/math.h>
-#include <units/torque.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/control/config/soft_limits.h"
 #include "frc846/robot/GenericSubsystem.h"
 #include "frc846/wpilib/units.h"
 #include "ports.h"
@@ -47,6 +42,7 @@ class ElevatorSubsystem
 private:
   bool hasZeroed = false;
 
+  // TODO: Set to correct reduction later
   elevator_pos_conv_t elevator_reduction_ = 1.0_in / 1.0_tr;
 
   frc846::control::config::MotorConstructionParameters motor_configs;