created subsystem skeleton

src/main/cpp/RobotContainer.cpp

@@ -89,6 +89,8 @@ SwerveSubsystem &RobotContainer::GetSwerveSubsystem() {
   return swerveSubsystem;
 }
 
+PivotSubsystem &RobotContainer::GetPivotSubsystem() { return pivotSubsystem; }
+
 str::Vision &RobotContainer::GetVision() { return vision; }
 
 str::NoteVisualizer &RobotContainer::GetNoteVisualizer() {

src/main/cpp/subsystems/PivotSubsystem.cpp

@@ -0,0 +1,90 @@
+// Copyright (c) FRC 2053.
+// Open Source Software; you can modify and/or share it under the terms of
+// the MIT License file in the root of this project
+
+#include "subsystems/PivotSubsystem.h"
+
+PivotSubsystem::PivotSubsystem() {
+  ConfigurePivotMotors();
+  ConfigurePivotEncoder();
+}
+
+// This method will be called once per scheduler run
+void PivotSubsystem::Periodic() {}
+
+bool PivotSubsystem::ConfigurePivotMotors() {
+  ctre::phoenix6::configs::TalonFXConfiguration pivotConfig{};
+  ctre::phoenix6::configs::Slot0Configs pivotSlotConfig{};
+
+  // Gains are dependent on control mode. By default, use TorqueCurrentFOC with
+  // Motion Magic Expo.
+  pivotSlotConfig.kA = consts::pivot::gains::kA.value();
+  pivotSlotConfig.kV = consts::pivot::gains::kV.value();
+  pivotSlotConfig.kS = consts::pivot::gains::kS.value();
+  pivotSlotConfig.kP = consts::pivot::gains::kP.value();
+  pivotSlotConfig.kI = consts::pivot::gains::kI.value();
+  pivotSlotConfig.kD = consts::pivot::gains::kD.value();
+  pivotSlotConfig.GravityType = ctre::phoenix6::signals::GravityTypeValue::Arm_Cosine;
+  pivotSlotConfig.kG = consts::pivot::gains::kG.value();
+  pivotConfig.Slot0 = pivotSlotConfig;
+
+  pivotConfig.MotorOutput.NeutralMode =
+      ctre::phoenix6::signals::NeutralModeValue::Brake;
+  pivotConfig.Feedback.FeedbackRemoteSensorID = pivotEncoder.GetDeviceID();
+  pivotConfig.Feedback.FeedbackSensorSource =
+      ctre::phoenix6::signals::FeedbackSensorSourceValue::FusedCANcoder;
+  pivotConfig.Feedback.RotorToSensorRatio =
+      consts::pivot::physical::PIVOT_GEARING;
+  pivotConfig.MotorOutput.Inverted = false;
+  pivotConfig.ClosedLoopGeneral.ContinuousWrap = false;
+
+  pivotConfig.MotionMagic.MotionMagicCruiseVelocity =
+      consts::pivot::gains::motionMagicCruiseVel.value();
+  // Motion magic expo kv and ka are always in terms of volts, even if we are
+  // controlling current.
+  pivotConfig.MotionMagic.MotionMagicExpo_kV =
+      consts::pivot::gains::motionMagicExpoKv.value();
+  pivotConfig.MotionMagic.MotionMagicExpo_kA =
+      consts::pivot::gains::motionMagicExpoKa.value();
+
+  pivotConfig.TorqueCurrent.PeakForwardTorqueCurrent =
+      consts::pivot::current_limits::STEER_TORQUE_CURRENT_LIMIT.value();
+  pivotConfig.TorqueCurrent.PeakReverseTorqueCurrent =
+      -consts::pivot::current_limits::STEER_TORQUE_CURRENT_LIMIT.value();
+
+  // Supply side limiting only effects non torque modes. We should set these
+  // anyway in case we want to control in a different mode
+  pivotConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
+  pivotConfig.CurrentLimits.SupplyCurrentLimit =
+      consts::pivot::current_limits::SUPPLY_CURRENT_LIMIT.value();
+
+  ctre::phoenix::StatusCode configResult =
+      pivotLeft.GetConfigurator().Apply(pivotConfig);
+
+  pivotRight.SetControl(
+      ctre::phoenix6::controls::Follower{pivotLeft.GetDeviceID(), true});
+
+  fmt::print("Configured pivot motors. Result was: {}\n",
+             configResult.GetName());
+
+  return configResult.IsOK();
+}
+
+bool PivotSubsystem::ConfigurePivotEncoder() {
+  ctre::phoenix6::configs::CANcoderConfiguration encoderConfig{};
+  encoderConfig.MagnetSensor.MagnetOffset =
+      consts::pivot::physical::ENCODER_OFFSET.value();
+  encoderConfig.MagnetSensor.AbsoluteSensorRange =
+      ctre::phoenix6::signals::AbsoluteSensorRangeValue::Unsigned_0To1;
+  encoderConfig.MagnetSensor.SensorDirection =
+      ctre::phoenix6::signals::SensorDirectionValue::CounterClockwise_Positive;
+  ctre::phoenix::StatusCode configResult =
+      pivotEncoder.GetConfigurator().Apply(encoderConfig);
+
+  fmt::print("Configured pivot encoder. Result was: {}\n",
+             configResult.GetName());
+
+  return configResult.IsOK();
+}
+
+void PivotSubsystem::ConfigureControlSignals() {}

src/main/deploy/branch.txt

@@ -1 +1 @@
-main
+pivot

src/main/deploy/commit.txt

@@ -1 +1 @@
-8de98bf
+93b0a46

src/main/include/Autos.h

@@ -9,11 +9,13 @@
 #include <frc2/command/Commands.h>
 #include <pathplanner/lib/auto/NamedCommands.h>
 #include <pathplanner/lib/commands/PathPlannerAuto.h>
+#include <subsystems/PivotSubsystem.h>
 #include <subsystems/SwerveSubsystem.h>
 
 class Autos {
 public:
-  explicit Autos(SwerveSubsystem &swerveSub) : swerveSub(swerveSub) {
+  explicit Autos(SwerveSubsystem &swerveSub, PivotSubsystem &pivotSub)
+      : swerveSub(swerveSub), pivotSub(pivotSub) {
     pathplanner::NamedCommands::registerCommand(
         "Shoot", frc2::cmd::Print("Shooting Note"));
 
@@ -49,6 +51,7 @@ class Autos {
   frc::SendableChooser<AutoSelector> autoChooser;
 
   SwerveSubsystem &swerveSub;
+  PivotSubsystem &pivotSub;
 
   frc2::CommandPtr selectCommand{frc2::cmd::None()};
 };

src/main/include/RobotContainer.h

@@ -10,6 +10,7 @@
 #include "Autos.h"
 #include "str/NoteVisualizer.h"
 #include "str/Vision.h"
+#include "subsystems/PivotSubsystem.h"
 #include "subsystems/SwerveSubsystem.h"
 
 class RobotContainer {
@@ -19,6 +20,7 @@ class RobotContainer {
   frc2::Command *GetAutonomousCommand();
 
   SwerveSubsystem &GetSwerveSubsystem();
+  PivotSubsystem &GetPivotSubsystem();
   str::Vision &GetVision();
   str::NoteVisualizer &GetNoteVisualizer();
 
@@ -30,5 +32,7 @@ class RobotContainer {
   str::Vision vision;
   str::NoteVisualizer noteVisualizer;
 
-  Autos autos{swerveSubsystem};
+  PivotSubsystem pivotSubsystem;
+
+  Autos autos{swerveSubsystem, pivotSubsystem};
 };

src/main/include/constants/PivotConstants.h

@@ -0,0 +1,40 @@
+// Copyright (c) FRC 2053.
+// Open Source Software; you can modify and/or share it under the terms of
+// the MIT License file in the root of this project
+
+#pragma once
+
+#include "str/Gains.h"
+
+namespace consts {
+namespace pivot {
+namespace can_ids {
+inline constexpr int LEFT_PIVOT = 15;
+inline constexpr int RIGHT_PIVOT = 16;
+inline constexpr int PIVOT_ENCODER = 17;
+} // namespace can_ids
+
+namespace current_limits {
+inline constexpr units::ampere_t STEER_TORQUE_CURRENT_LIMIT = 40_A;
+inline constexpr units::ampere_t SUPPLY_CURRENT_LIMIT = 60_A;
+} // namespace current_limits
+
+namespace physical {
+inline constexpr units::scalar_t PIVOT_GEARING = 100.0 / 1.0;
+inline constexpr units::turn_t ENCODER_OFFSET = 0.1_tr;
+} // namespace physical
+
+namespace gains {
+inline constexpr units::turns_per_second_t motionMagicCruiseVel = 90_deg_per_s;
+inline constexpr str::gains::radial::turn_volt_ka_unit_t motionMagicExpoKa{0};
+inline constexpr str::gains::radial::turn_volt_kv_unit_t motionMagicExpoKv{0};
+inline constexpr str::gains::radial::turn_amp_ka_unit_t kA{0};
+inline constexpr str::gains::radial::turn_amp_kv_unit_t kV{0};
+inline constexpr units::ampere_t kS{0};
+inline constexpr units::ampere_t kG{0};
+inline constexpr str::gains::radial::turn_amp_kp_unit_t kP{0};
+inline constexpr str::gains::radial::turn_amp_ki_unit_t kI{0};
+inline constexpr str::gains::radial::turn_amp_kd_unit_t kD{0};
+} // namespace gains
+} // namespace pivot
+} // namespace consts

src/main/include/subsystems/PivotSubsystem.h

@@ -0,0 +1,34 @@
+// Copyright (c) FRC 2053.
+// Open Source Software; you can modify and/or share it under the terms of
+// the MIT License file in the root of this project
+
+#pragma once
+
+#include <frc2/command/SubsystemBase.h>
+
+#include <ctre/phoenix6/CANcoder.hpp>
+#include <ctre/phoenix6/TalonFX.hpp>
+
+#include "constants/PivotConstants.h"
+
+class PivotSubsystem : public frc2::SubsystemBase {
+public:
+  PivotSubsystem();
+
+  /**
+   * Will be called periodically whenever the CommandScheduler runs.
+   */
+  void Periodic() override;
+
+private:
+  bool ConfigurePivotMotors();
+  bool ConfigurePivotEncoder();
+  void ConfigureControlSignals();
+
+  ctre::phoenix6::hardware::TalonFX pivotLeft{
+      consts::pivot::can_ids::LEFT_PIVOT};
+  ctre::phoenix6::hardware::TalonFX pivotRight{
+      consts::pivot::can_ids::RIGHT_PIVOT};
+  ctre::phoenix6::hardware::CANcoder pivotEncoder{
+      consts::pivot::can_ids::PIVOT_ENCODER};
+};