From 7ddd6d133d5676b52b7e7dc238a9634c57f31fbc Mon Sep 17 00:00:00 2001 From: prak132 Date: Tue, 14 Jan 2025 22:38:59 -0800 Subject: [PATCH 1/4] Added algae pivot subsystem and updated files paths to remove redundant included files --- .../cpp/subsystems/hardware/algae_pivot.cc | 62 +++++++++++++++++++ src/y2025/cpp/subsystems/hardware/elevator.cc | 4 +- src/y2025/include/ports.h | 4 ++ .../include/subsystems/SubsystemHelper.h | 3 +- .../include/subsystems/hardware/algae_pivot.h | 56 +++++++++++++++++ .../include/subsystems/hardware/elevator.h | 6 +- 6 files changed, 126 insertions(+), 9 deletions(-) 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..dbd00a1 --- /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( + "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("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("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..75cc550 100644 --- 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 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/SubsystemHelper.h b/src/y2025/include/subsystems/SubsystemHelper.h index f34ca62..a89c3ae 100644 --- a/src/y2025/include/subsystems/SubsystemHelper.h +++ b/src/y2025/include/subsystems/SubsystemHelper.h @@ -3,8 +3,7 @@ #include #include -#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 new file mode 100644 index 0000000..028b31b --- /dev/null +++ b/src/y2025/include/subsystems/hardware/algae_pivot.h @@ -0,0 +1,56 @@ +#pragma once + +#include + +#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>>; + +class AlgaePivotSubsystem + : public frc846::robot::GenericSubsystem { +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( + "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 motor_helper_; + + AlgaePivotReadings ReadFromHardware() override; + + void WriteToHardware(AlgaePivotTarget target) override; +}; \ No newline at end of file diff --git a/src/y2025/include/subsystems/hardware/elevator.h b/src/y2025/include/subsystems/hardware/elevator.h index 2223309..45275f8 100644 --- a/src/y2025/include/subsystems/hardware/elevator.h +++ b/src/y2025/include/subsystems/hardware/elevator.h @@ -1,18 +1,13 @@ #pragma once -#include -#include #include #include -#include -#include #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; From 686fd3e6968fa9bbef11f68701560efb0b2d55cd Mon Sep 17 00:00:00 2001 From: prak132 Date: Wed, 15 Jan 2025 16:41:43 -0800 Subject: [PATCH 2/4] Added vertical arm control to algae pivot and fixed issues with vertical arm calculator --- .../robot/calculators/VerticalArmCalculator.cc | 15 +++++---------- .../robot/calculators/VerticalArmCalculator.h | 11 ++--------- src/y2025/cpp/subsystems/hardware/algae_pivot.cc | 6 +++++- src/y2025/include/subsystems/SubsystemHelper.h | 8 ++++---- .../include/subsystems/hardware/algae_pivot.h | 3 +++ 5 files changed, 19 insertions(+), 24 deletions(-) diff --git a/src/frc846/cpp/frc846/robot/calculators/VerticalArmCalculator.cc b/src/frc846/cpp/frc846/robot/calculators/VerticalArmCalculator.cc index 52f9c6d..e8ab0d7 100644 --- a/src/frc846/cpp/frc846/robot/calculators/VerticalArmCalculator.cc +++ b/src/frc846/cpp/frc846/robot/calculators/VerticalArmCalculator.cc @@ -1,21 +1,16 @@ #include "frc846/robot/calculators/VerticalArmCalculator.h" -#include -#include #include 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 +18,8 @@ double VerticalArmCalculator::calculate(VerticalArmInputs inputs) { double duty_cycle = inputs.motor_gains.calculate(position_error.to(), 0.0, inputs.current_velocity.to(), gravity_torque.to()); - 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..ffa227e 100644 --- a/src/frc846/include/frc846/robot/calculators/VerticalArmCalculator.h +++ b/src/frc846/include/frc846/robot/calculators/VerticalArmCalculator.h @@ -4,11 +4,11 @@ #include #include #include +#include #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,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 { public: - VerticalArmCalculator(VerticalArmConfigs& configs); + VerticalArmCalculator(); /* calculate() @@ -76,9 +72,6 @@ class VerticalArmCalculator : public frc846::math::Calculator motor_helper_; + frc846::robot::calculators::VerticalArmCalculator arm_calculator_; + AlgaePivotReadings ReadFromHardware() override; void WriteToHardware(AlgaePivotTarget target) override; From 47aea18943608ab24eae4211bfa74dcecf4e6032 Mon Sep 17 00:00:00 2001 From: prak132 Date: Thu, 16 Jan 2025 22:51:39 -0800 Subject: [PATCH 3/4] Added ramp subsystem and minor fixes to algae pivot --- .../cpp/subsystems/hardware/algae_pivot.cc | 6 +- src/y2025/cpp/subsystems/hardware/ramp.cc | 49 ++++++++++++++++ src/y2025/include/ports.h | 4 ++ src/y2025/include/subsystems/hardware/ramp.h | 57 +++++++++++++++++++ 4 files changed, 113 insertions(+), 3 deletions(-) create mode 100644 src/y2025/cpp/subsystems/hardware/ramp.cc create mode 100644 src/y2025/include/subsystems/hardware/ramp.h diff --git a/src/y2025/cpp/subsystems/hardware/algae_pivot.cc b/src/y2025/cpp/subsystems/hardware/algae_pivot.cc index a7f65b3..ea03b1e 100644 --- a/src/y2025/cpp/subsystems/hardware/algae_pivot.cc +++ b/src/y2025/cpp/subsystems/hardware/algae_pivot.cc @@ -18,9 +18,9 @@ AlgaePivotSubsystem::AlgaePivotSubsystem() REGISTER_SOFTLIMIT_CONFIG("algae_pivot/algae_pivot_softlimits", true, 1.0); // bool using_limits = - // GetPreferenceValue_bool("elevator/elevator_softlimits/using_limits"); + // GetPreferenceValue_bool("algae_pivot/algae_pivot_softlimits/using_limits"); // double reduce_max_dc = - // GetPreferenceValue_double("elevator/elevator_softlimits/reduce_max_dc"); + // GetPreferenceValue_double("algae_pivot/algae_pivot_softlimits/reduce_max_dc"); motor_helper_.SetConversion(algae_pivot_reduction_); @@ -47,7 +47,7 @@ AlgaePivotTarget AlgaePivotSubsystem::ZeroTarget() const { bool AlgaePivotSubsystem::VerifyHardware() { bool ok = true; FRC846_VERIFY( - pivot_.VerifyConnected(), ok, "Could not verify elevator motor"); + pivot_.VerifyConnected(), ok, "Could not verify algae pivot motor"); return ok; } diff --git a/src/y2025/cpp/subsystems/hardware/ramp.cc b/src/y2025/cpp/subsystems/hardware/ramp.cc new file mode 100644 index 0000000..aa4df2f --- /dev/null +++ b/src/y2025/cpp/subsystems/hardware/ramp.cc @@ -0,0 +1,49 @@ +#include "subsystems/hardware/ramp.h" + +#include "subsystems/SubsystemHelper.h" + +RampSubsystem::RampSubsystem() + : frc846::robot::GenericSubsystem("ramp"), + motor_configs(GET_MOTOR_CONFIG("ramp/ramp_", ports::ramp_::kRamp_CANID, + frc846::wpilib::unit_ohm{0.0}, frc846::wpilib::unit_kg_m_sq{0.0})), + ramp_(frc846::control::base::MotorMonkeyType::SPARK_MAX_VORTEX, + motor_configs) { + REGISTER_MOTOR_CONFIG("ramp/ramp_", false, true, 40_A, 40_A, 16.0_V); + REGISTER_PIDF_CONFIG("ramp/ramp_gains_", 0.0, 0.0, 0.0, 0.0); + + motor_helper_.SetConversion(ramp_reduction_); + motor_helper_.bind(&ramp_); +} + +void RampSubsystem::Setup() { + ramp_.Setup(); + ramp_.EnableStatusFrames( + {frc846::control::config::StatusFrame::kVelocityFrame}); +} + +RampTarget RampSubsystem::ZeroTarget() const { + return RampTarget{RampState::kIdle, 0.0_fps}; +} + +bool RampSubsystem::VerifyHardware() { + bool ok = true; + FRC846_VERIFY(ramp_.VerifyConnected(), ok, "Could not verify ramp motor"); + return ok; +} + +RampReadings RampSubsystem::ReadFromHardware() { + RampReadings readings; + readings.vel = motor_helper_.GetVelocity(); + Graph("readings/ramp_velocity", readings.vel); + return readings; +} + +void RampSubsystem::WriteToHardware(RampTarget target) { + ramp_.SetGains(GET_PIDF_GAINS("ramp/ramp_gains_")); + if (target.state == RampState::kIntake) { + target.vel = 2.0_fps; + } else { + target.vel = 0.0_fps; + } + motor_helper_.WriteVelocity(target.vel); +} \ No newline at end of file diff --git a/src/y2025/include/ports.h b/src/y2025/include/ports.h index 738ec0f..427ca0a 100644 --- a/src/y2025/include/ports.h +++ b/src/y2025/include/ports.h @@ -38,4 +38,8 @@ struct ports { struct algae_pivot_ { static constexpr int kAlgaePivotOne_CANID = 18; }; + + struct ramp_ { + static constexpr int kRamp_CANID = 20; + }; }; diff --git a/src/y2025/include/subsystems/hardware/ramp.h b/src/y2025/include/subsystems/hardware/ramp.h new file mode 100644 index 0000000..86a37a3 --- /dev/null +++ b/src/y2025/include/subsystems/hardware/ramp.h @@ -0,0 +1,57 @@ +#pragma once + +#include +#include + +#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/robot/calculators/VerticalArmCalculator.h" +#include "frc846/wpilib/units.h" +#include "ports.h" + +enum RampState { kIntake, kIdle }; + +struct RampReadings { + RampState state; + units::feet_per_second_t vel; +}; + +struct RampTarget { + RampState state; + units::feet_per_second_t vel; +}; + +using ramp_pos_conv_t = units::unit_t< + units::compound_unit>>; + +class RampSubsystem + : public frc846::robot::GenericSubsystem { +public: + RampSubsystem(); + + void Setup() override; + + RampTarget ZeroTarget() const override; + + bool VerifyHardware() override; + +private: + bool hasZeroed = false; + + // TODO: Set to correct reduction later + ramp_pos_conv_t ramp_reduction_ = 1.0_ft / 1.0_tr; + + frc846::control::config::MotorConstructionParameters motor_configs; + frc846::control::HigherMotorController ramp_; + frc846::control::HMCHelper motor_helper_; + + frc846::robot::calculators::VerticalArmCalculator arm_calculator_; + + RampReadings ReadFromHardware() override; + + void WriteToHardware(RampTarget target) override; +}; \ No newline at end of file From 8caecb8e326db1cbd6acebf692967b89a0325f45 Mon Sep 17 00:00:00 2001 From: DhananjayKhulbe Date: Sun, 19 Jan 2025 16:57:55 -0800 Subject: [PATCH 4/4] Revert "Merge branch 'merged_subsystems' into elevator_subsystem" This reverts commit ff004d91275bd7d31d6e4a31343b5da3476fcd94, reversing changes made to 47aea18943608ab24eae4211bfa74dcecf4e6032. this reverts the merge --- README.md | 2 - src/y2025/cpp/subsystems/hardware/climber.cc | 76 ------------------- .../subsystems/hardware/coral_telescope.cc | 57 -------------- .../cpp/subsystems/hardware/coral_wrist.cc | 55 -------------- src/y2025/include/ports.h | 12 +-- .../include/subsystems/hardware/climber.h | 58 -------------- .../subsystems/hardware/coral_telescope.h | 53 ------------- .../include/subsystems/hardware/coral_wrist.h | 53 ------------- 8 files changed, 1 insertion(+), 365 deletions(-) delete mode 100644 src/y2025/cpp/subsystems/hardware/climber.cc delete mode 100644 src/y2025/cpp/subsystems/hardware/coral_telescope.cc delete mode 100644 src/y2025/cpp/subsystems/hardware/coral_wrist.cc delete mode 100644 src/y2025/include/subsystems/hardware/climber.h delete mode 100644 src/y2025/include/subsystems/hardware/coral_telescope.h delete mode 100644 src/y2025/include/subsystems/hardware/coral_wrist.h diff --git a/README.md b/README.md index 8f34603..ac52f3e 100644 --- a/README.md +++ b/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] diff --git a/src/y2025/cpp/subsystems/hardware/climber.cc b/src/y2025/cpp/subsystems/hardware/climber.cc deleted file mode 100644 index b09b908..0000000 --- a/src/y2025/cpp/subsystems/hardware/climber.cc +++ /dev/null @@ -1,76 +0,0 @@ -#include "subsystems/hardware/climber.h" - -#include "subsystems/SubsystemHelper.h" -#include "subsystems/hardware/climber.h" - -ClimberSubsystem::ClimberSubsystem() - : frc846::robot::GenericSubsystem( - "climber"), - motor_configs(GET_MOTOR_CONFIG("climber/climber_one_", - ports::climber_::kClimberOne_CANID, frc846::wpilib::unit_ohm{0.0}, - frc846::wpilib::unit_kg_m_sq{0.0})), - motor_two_configs(GET_MOTOR_CONFIG("climber/climber_two_", - ports::climber_::kClimberOne_CANID, frc846::wpilib::unit_ohm{0.0}, - frc846::wpilib::unit_kg_m_sq{0.0})), - climber_(frc846::control::base::MotorMonkeyType::SPARK_MAX_NEO550, - motor_configs), - climber_two_(frc846::control::base::MotorMonkeyType::SPARK_MAX_NEO550, - motor_two_configs) { - RegisterPreference("climber/climber_tolerance_", 0.25_in); - - REGISTER_MOTOR_CONFIG( - "climber/climber_one_", false, true, 40_A, 40_A, 16.0_V); - REGISTER_MOTOR_CONFIG( - "climber/climber_two_", false, true, 40_A, 40_A, 16.0_V); - REGISTER_PIDF_CONFIG("climber/climber_gains_", 0.0, 0.0, 0.0, 0.0); - REGISTER_SOFTLIMIT_CONFIG("climber/climber_softlimits", true, 1.0); - - motor_helper_.SetConversion(climber_reduction_); - motor_helper_two_.SetConversion(climber_reduction_); - motor_helper_.bind(&climber_); - motor_helper_two_.bind(&climber_two_); -} - -void ClimberSubsystem::Setup() { climber_.Setup(); } - -ClimberTarget ClimberSubsystem::ZeroTarget() const { - ClimberTarget target; - target.position = 0.0_deg; - return target; -} - -bool ClimberSubsystem::VerifyHardware() { - bool ok = true; - - FRC846_VERIFY(climber_.VerifyConnected(), ok, "climber one not connected"); - FRC846_VERIFY( - climber_two_.VerifyConnected(), ok, "climber two not connected"); - - return ok; - return true; -} - -ClimberReadings ClimberSubsystem::ReadFromHardware() { - ClimberReadings readings; - readings.position = motor_helper_.GetPosition(); - - Graph("Climber position", readings.position); - - return readings; -} - -void ClimberSubsystem::WriteToHardware(ClimberTarget target) { - climber_.SetGains(GET_PIDF_GAINS("climber/climber_gains_")); - climber_two_.SetGains(GET_PIDF_GAINS("climber/climber_two_gains_")); - - if (target.target_state == kPreClimb) { - target.position = 30.0_deg; - } else if (target.target_state == kClimb) { - target.position = 60.0_deg; - } else { - target.position = 90.0_deg; - } - - motor_helper_.WritePosition(target.position); - motor_helper_two_.WritePosition(target.position); -} diff --git a/src/y2025/cpp/subsystems/hardware/coral_telescope.cc b/src/y2025/cpp/subsystems/hardware/coral_telescope.cc deleted file mode 100644 index 0f4cfbb..0000000 --- a/src/y2025/cpp/subsystems/hardware/coral_telescope.cc +++ /dev/null @@ -1,57 +0,0 @@ -#include "subsystems/hardware/coral_telescope.h" - -#include "subsystems/SubsystemHelper.h" - -TelescopeSubsystem::TelescopeSubsystem() - : frc846::robot::GenericSubsystem( - "telescope"), - motor_configs(GET_MOTOR_CONFIG("telescope/telescope_one_", - ports::telescope_::kTelescopeOne_CANID, frc846::wpilib::unit_ohm{0.0}, - frc846::wpilib::unit_kg_m_sq{0.0})), - telescope_(frc846::control::base::MotorMonkeyType::TALON_FX_KRAKENX60, - motor_configs) { - RegisterPreference("telescope/telescope_tolerance_", 0.25_in); - - REGISTER_MOTOR_CONFIG( - "telescope/telescope_one_", false, true, 40_A, 40_A, 16.0_V); - REGISTER_PIDF_CONFIG("telescope/telescope_gains_", 0.0, 0.0, 0.0, 0.0); - REGISTER_SOFTLIMIT_CONFIG("telescope/telescope_softlimits", true, 1.0); - - motor_helper_.SetConversion(telescope_reduction); - - motor_helper_.bind(&telescope_); -} - -void TelescopeSubsystem::Setup() { telescope_.Setup(); } - -TelescopeTarget TelescopeSubsystem::ZeroTarget() const { - TelescopeTarget target; - target.extension = 0.0_in; - return target; -} - -bool TelescopeSubsystem::VerifyHardware() { - if (is_initialized()) { - bool ok = true; - - FRC846_VERIFY( - telescope_.VerifyConnected(), ok, "Telescope ESC not connected"); - - return ok; - } - return true; -} - -TelescopeReadings TelescopeSubsystem::ReadFromHardware() { - TelescopeReadings readings; - readings.extension = motor_helper_.GetPosition(); - - Graph("Telescope extension", readings.extension); - - return readings; -} - -void TelescopeSubsystem::WriteToHardware(TelescopeTarget target) { - telescope_.SetGains(GET_PIDF_GAINS("telescope/telescope_gains_")); - motor_helper_.WritePosition(target.extension); -} diff --git a/src/y2025/cpp/subsystems/hardware/coral_wrist.cc b/src/y2025/cpp/subsystems/hardware/coral_wrist.cc deleted file mode 100644 index bb1c331..0000000 --- a/src/y2025/cpp/subsystems/hardware/coral_wrist.cc +++ /dev/null @@ -1,55 +0,0 @@ -#include "subsystems/hardware/coral_wrist.h" - -#include "subsystems/SubsystemHelper.h" - -CoralWristSubsystem::CoralWristSubsystem() - : frc846::robot::GenericSubsystem( - "coral_wrist"), - motor_configs(GET_MOTOR_CONFIG("coral_wrist/coral_wrist_one_", - ports::coral_wrist_::kCoralWristOne_CANID, - frc846::wpilib::unit_ohm{0.0}, frc846::wpilib::unit_kg_m_sq{0.0})), - coral_wrist_(frc846::control::base::MotorMonkeyType::SPARK_MAX_NEO550, - motor_configs) { - RegisterPreference("coral_wrist/coral_wrist_tolerance_", 0.25_in); - - REGISTER_MOTOR_CONFIG( - "coral_wrist/coral_wrist_one_", false, true, 40_A, 40_A, 16.0_V); - REGISTER_PIDF_CONFIG("coral_wrist/coral_wrist_gains_", 0.0, 0.0, 0.0, 0.0); - REGISTER_SOFTLIMIT_CONFIG("coral_wrist/coral_wrist_softlimits", true, 1.0); - - motor_helper_.SetConversion(coral_wrist_reduction); - - motor_helper_.bind(&coral_wrist_); -} - -void CoralWristSubsystem::Setup() { coral_wrist_.Setup(); } - -CoralWristTarget CoralWristSubsystem::ZeroTarget() const { - CoralWristTarget target; - target.position = 0.0_deg; - return target; -} - -bool CoralWristSubsystem::VerifyHardware() { - bool ok = true; - - FRC846_VERIFY( - coral_wrist_.VerifyConnected(), ok, "Coral Wrist not connected"); - - return ok; - return true; -} - -CoralWristReadings CoralWristSubsystem::ReadFromHardware() { - CoralWristReadings readings; - readings.position = motor_helper_.GetPosition(); - - Graph("Coral Wrist extension", readings.position); - - return readings; -} - -void CoralWristSubsystem::WriteToHardware(CoralWristTarget target) { - coral_wrist_.SetGains(GET_PIDF_GAINS("coral_wrist/coral_wrist_gains_")); - motor_helper_.WritePosition(target.position); -} diff --git a/src/y2025/include/ports.h b/src/y2025/include/ports.h index 7dcbbef..427ca0a 100644 --- a/src/y2025/include/ports.h +++ b/src/y2025/include/ports.h @@ -34,22 +34,12 @@ struct ports { struct elevator_ { static constexpr int kElevatorOne_CANID = 15; }; + struct algae_pivot_ { static constexpr int kAlgaePivotOne_CANID = 18; }; struct ramp_ { static constexpr int kRamp_CANID = 20; - struct telescope_ { - static constexpr int kTelescopeOne_CANID = 16; - }; - - struct coral_wrist_ { - static constexpr int kCoralWristOne_CANID = 16; - }; - - struct climber_ { - static constexpr int kClimberOne_CANID = 17; - static constexpr int kClimberTwo_CANID = 18; }; }; diff --git a/src/y2025/include/subsystems/hardware/climber.h b/src/y2025/include/subsystems/hardware/climber.h deleted file mode 100644 index 0b229f8..0000000 --- a/src/y2025/include/subsystems/hardware/climber.h +++ /dev/null @@ -1,58 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include - -#include "frc846/base/Loggable.h" -#include "frc846/control/HMCHelper.h" -#include "frc846/control/HigherMotorController.h" -#include "frc846/robot/GenericSubsystem.h" -#include "ports.h" -#include "units/length.h" -#include "units/math.h" - -struct ClimberReadings { - units::degree_t position; -}; - -enum ClimberState { kIdle, kPreClimb, kClimb }; - -struct ClimberTarget { - units::degree_t position; - ClimberState target_state; -}; - -using climber_pos_conv_t = units::unit_t< - units::compound_unit>>; - -class ClimberSubsystem - : public frc846::robot::GenericSubsystem { -public: - ClimberSubsystem(); - - void Setup() override; - - ClimberTarget ZeroTarget() const override; - - bool VerifyHardware() override; - -private: - ClimberReadings ReadFromHardware() override; - - void WriteToHardware(ClimberTarget target) override; - - climber_pos_conv_t climber_reduction_ = 90.0_deg / 1.0_tr; - - frc846::control::config::MotorConstructionParameters motor_configs; - frc846::control::config::MotorConstructionParameters motor_two_configs; - frc846::control::HigherMotorController climber_; - frc846::control::HigherMotorController climber_two_; - frc846::control::HMCHelper motor_helper_; - frc846::control::HMCHelper motor_helper_two_; - - bool has_zeroed_ = false; -}; diff --git a/src/y2025/include/subsystems/hardware/coral_telescope.h b/src/y2025/include/subsystems/hardware/coral_telescope.h deleted file mode 100644 index 318b9b4..0000000 --- a/src/y2025/include/subsystems/hardware/coral_telescope.h +++ /dev/null @@ -1,53 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include - -#include "frc846/base/Loggable.h" -#include "frc846/control/HMCHelper.h" -#include "frc846/control/HigherMotorController.h" -#include "frc846/robot/GenericSubsystem.h" -#include "ports.h" -#include "units/length.h" -#include "units/math.h" - -struct TelescopeReadings { - units::inch_t extension; -}; - -struct TelescopeTarget { - units::inch_t extension; -}; - -using telescope_pos_conv_t = units::unit_t< - units::compound_unit>>; - -class TelescopeSubsystem - : public frc846::robot::GenericSubsystem { -public: - TelescopeSubsystem(); - - void Setup() override; - - TelescopeTarget ZeroTarget() const override; - - bool VerifyHardware() override; - -private: - TelescopeReadings ReadFromHardware() override; - - void WriteToHardware(TelescopeTarget target) override; - - telescope_pos_conv_t telescope_reduction = 1.0_in / 0.0_tr; - - frc846::control::config::MotorConstructionParameters motor_configs; - frc846::control::HigherMotorController telescope_; - frc846::control::HMCHelper motor_helper_; - - bool has_zeroed_ = false; -}; diff --git a/src/y2025/include/subsystems/hardware/coral_wrist.h b/src/y2025/include/subsystems/hardware/coral_wrist.h deleted file mode 100644 index 531cf52..0000000 --- a/src/y2025/include/subsystems/hardware/coral_wrist.h +++ /dev/null @@ -1,53 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include - -#include "frc846/base/Loggable.h" -#include "frc846/control/HMCHelper.h" -#include "frc846/control/HigherMotorController.h" -#include "frc846/robot/GenericSubsystem.h" -#include "ports.h" -#include "units/length.h" -#include "units/math.h" - -struct CoralWristReadings { - units::degree_t position; -}; - -struct CoralWristTarget { - units::degree_t position; -}; - -using CoralWrist_pos_conv_t = units::unit_t< - units::compound_unit>>; - -class CoralWristSubsystem - : public frc846::robot::GenericSubsystem { -public: - CoralWristSubsystem(); - - void Setup() override; - - CoralWristTarget ZeroTarget() const override; - - bool VerifyHardware() override; - -private: - CoralWristReadings ReadFromHardware() override; - - void WriteToHardware(CoralWristTarget target) override; - - CoralWrist_pos_conv_t coral_wrist_reduction = 1.0_deg / 1.0_tr; - - frc846::control::config::MotorConstructionParameters motor_configs; - frc846::control::HigherMotorController coral_wrist_; - frc846::control::HMCHelper motor_helper_; - - bool has_zeroed_ = false; -};