added intake motor

src/main/cpp/RobotContainer.cpp

@@ -62,15 +62,6 @@ void RobotContainer::ConfigureBindings() {
   // (controller.X() && controller.Back()).WhileTrue(shooterSubsystem.BottomWheelSysIdDynamic(frc2::sysid::Direction::kForward));
   // (controller.Y() && controller.Back()).WhileTrue(shooterSubsystem.BottomWheelSysIdDynamic(frc2::sysid::Direction::kReverse));
 
-  // controller.A().WhileTrue(swerveSubsystem.SysIdSteerMk4nQuasistaticTorque(
-  //     frc2::sysid::Direction::kForward));
-  // controller.B().WhileTrue(swerveSubsystem.SysIdSteerMk4nQuasistaticTorque(
-  //     frc2::sysid::Direction::kReverse));
-  // controller.X().WhileTrue(swerveSubsystem.SysIdSteerMk4nDynamicTorque(
-  //     frc2::sysid::Direction::kForward));
-  // controller.Y().WhileTrue(swerveSubsystem.SysIdSteerMk4nDynamicTorque(
-  //     frc2::sysid::Direction::kReverse));
-
   // controller.A().WhileTrue(swerveSubsystem.SysIdSteerMk4iQuasistaticTorque(frc2::sysid::Direction::kForward));
   // controller.B().WhileTrue(swerveSubsystem.SysIdSteerMk4iQuasistaticTorque(frc2::sysid::Direction::kReverse));
   // controller.X().WhileTrue(swerveSubsystem.SysIdSteerMk4iDynamicTorque(frc2::sysid::Direction::kForward));
@@ -106,6 +97,10 @@ ShooterSubsystem &RobotContainer::GetShooterSubsystem() {
   return shooterSubsystem;
 }
 
+IntakeSubsystem &RobotContainer::GetIntakeSubsystem() {
+  return intakeSubsystem;
+}
+
 str::Vision &RobotContainer::GetVision() { return vision; }
 
 str::NoteVisualizer &RobotContainer::GetNoteVisualizer() {

src/main/cpp/subsystems/IntakeSubsystem.cpp

@@ -4,12 +4,12 @@
 
 #include "subsystems/IntakeSubsystem.h"
 #include <frc/smartdashboard/SmartDashboard.h>
+#include "constants/Constants.h"
 
 IntakeSubsystem::IntakeSubsystem() {
-
+    SetName("IntakeSubsystem");
     ConfigureIntakeMotor(consts::intake::physical::INVERT_MOTOR, consts::intake::physical::INTAKE_RATIO, consts::intake::current_limits::SUPPLY_CURRENT_LIMIT, consts::intake::current_limits::STATOR_CURRENT_LIMIT);
     ConfigureMotorSignals();
-    SetName("IntakeSubsystem");
     frc::SmartDashboard::PutData(this);
 }
 
@@ -20,10 +20,46 @@ bool IntakeSubsystem::ConfigureIntakeMotor(bool invert,
                             units::scalar_t intakeGearing,
                             units::ampere_t supplyCurrentLimit,
                             units::ampere_t statorCurrentLimit) {
-                                return true;
-                            }
+
+  ctre::phoenix6::configs::TalonFXConfiguration intakeConfig{};
+
+  intakeConfig.MotorOutput.NeutralMode =
+      ctre::phoenix6::signals::NeutralModeValue::Brake;
+  intakeConfig.CurrentLimits.StatorCurrentLimit = statorCurrentLimit.value();
+  intakeConfig.CurrentLimits.StatorCurrentLimitEnable = true;
+
+  intakeConfig.MotorOutput.Inverted =
+      invert
+          ? ctre::phoenix6::signals::InvertedValue::Clockwise_Positive
+          : ctre::phoenix6::signals::InvertedValue::CounterClockwise_Positive;
+
+  intakeConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
+  intakeConfig.CurrentLimits.SupplyCurrentLimit = supplyCurrentLimit.value();
+
+  ctre::phoenix::StatusCode intakeConfigResult =
+      intakeMotor.GetConfigurator().Apply(intakeConfig);
+
+  fmt::print("Configured intake motor. Result was: {}\n",
+             intakeConfigResult.GetName());
+
+  return intakeConfigResult.IsOK();
+}
 
 
 bool IntakeSubsystem::ConfigureMotorSignals() {
-    return true;
+  intakeMotorVoltageSetter.UpdateFreqHz = 0_Hz;
+
+  //Double the rio update rate? Not sure what is optimal here
+  units::hertz_t updateRate = 1.0 / (consts::LOOP_PERIOD * 2.0);
+
+  intakeMotorVoltageSig.SetUpdateFrequency(updateRate);
+  intakeMotorTorqueCurrentSig.SetUpdateFrequency(updateRate);
+
+  ctre::phoenix::StatusCode optimizeIntakeMotor =
+      intakeMotor.OptimizeBusUtilization();
+  if (optimizeIntakeMotor.IsOK()) {
+    fmt::print("Optimized bus signals for intake motor\n");
+  }
+
+  return optimizeIntakeMotor.IsOK();
 }

src/main/deploy/commit.txt

@@ -1 +1 @@
-f5a5c55
+7857c51

src/main/include/Autos.h

@@ -11,11 +11,12 @@
 #include <pathplanner/lib/commands/PathPlannerAuto.h>
 #include <subsystems/ShooterSubsystem.h>
 #include <subsystems/SwerveSubsystem.h>
+#include <subsystems/IntakeSubsystem.h>
 
 class Autos {
 public:
-  explicit Autos(SwerveSubsystem &swerveSub, ShooterSubsystem &shooterSub)
-      : swerveSub(swerveSub), shooterSub(shooterSub) {
+  explicit Autos(SwerveSubsystem &swerveSub, ShooterSubsystem &shooterSub, IntakeSubsystem &intakeSub)
+      : swerveSub(swerveSub), shooterSub(shooterSub), intakeSub(intakeSub) {
     pathplanner::NamedCommands::registerCommand(
         "Shoot", frc2::cmd::Print("Shooting Note"));
 
@@ -52,6 +53,7 @@ class Autos {
 
   SwerveSubsystem &swerveSub;
   ShooterSubsystem &shooterSub;
+  IntakeSubsystem &intakeSub;
 
   frc2::CommandPtr selectCommand{frc2::cmd::None()};
 };

src/main/include/RobotContainer.h

@@ -20,6 +20,7 @@ class RobotContainer {
 
   SwerveSubsystem &GetSwerveSubsystem();
   ShooterSubsystem &GetShooterSubsystem();
+  IntakeSubsystem &GetIntakeSubsystem();
   str::Vision &GetVision();
   str::NoteVisualizer &GetNoteVisualizer();
 
@@ -29,8 +30,9 @@ class RobotContainer {
 
   SwerveSubsystem swerveSubsystem;
   ShooterSubsystem shooterSubsystem;
+  IntakeSubsystem intakeSubsystem;
   str::Vision vision;
   str::NoteVisualizer noteVisualizer;
 
-  Autos autos{swerveSubsystem, shooterSubsystem};
+  Autos autos{swerveSubsystem, shooterSubsystem, intakeSubsystem};
 };

src/main/include/constants/IntakeConstants.h

@@ -30,12 +30,12 @@ inline constexpr bool INVERT_MOTOR = false;
 
 inline constexpr units::scalar_t INTAKE_RATIO = (24.0 / 16.0);
 
-inline constexpr frc::DCMotor SHOOTER_MOTOR = frc::DCMotor::Falcon500(1);
+inline constexpr frc::DCMotor INTAKE_MOTOR = frc::DCMotor::Falcon500(1);
 
 inline constexpr units::meter_t WHEEL_RADIUS = 1_in;
 
 //From onshape doc
-inline constexpr units::kilogram_square_meter_t FLYWHEEL_MOI = 12.350445 * 1_in * 1_in * 1_lb;
+inline constexpr units::kilogram_square_meter_t INTAKE_MOI = 12.350445 * 1_in * 1_in * 1_lb;
 } // namespace physical
 } // namespace intake
 } // namespace consts

src/main/include/subsystems/IntakeSubsystem.h

@@ -10,6 +10,9 @@
 #include <networktables/NetworkTableInstance.h>
 #include <networktables/DoubleTopic.h>
 #include <constants/IntakeConstants.h>
+#include <ctre/phoenix6/TalonFX.hpp>
+#include <frc/system/plant/LinearSystemId.h>
+#include <frc/simulation/FlywheelSim.h>
 
 class IntakeSubsystem : public frc2::SubsystemBase {
  public:
@@ -29,6 +32,18 @@ class IntakeSubsystem : public frc2::SubsystemBase {
                               units::ampere_t statorCurrentLimit);
   bool ConfigureMotorSignals();
 
+  ctre::phoenix6::hardware::TalonFX intakeMotor{consts::intake::can_ids::INTAKE, "*"};
+  ctre::phoenix6::StatusSignal<units::volt_t> intakeMotorVoltageSig = intakeMotor.GetMotorVoltage();
+  ctre::phoenix6::StatusSignal<units::ampere_t> intakeMotorTorqueCurrentSig = intakeMotor.GetTorqueCurrent();
+  ctre::phoenix6::controls::VoltageOut intakeMotorVoltageSetter{0_V};
+
+  ctre::phoenix6::sim::TalonFXSimState &intakeMotorSim = intakeMotor.GetSimState();
+
+  frc::LinearSystem<1, 1, 1>  intakePlant{frc::LinearSystemId::FlywheelSystem(
+      consts::intake::physical::INTAKE_MOTOR, consts::intake::physical::INTAKE_MOI, consts::intake::physical::INTAKE_RATIO)};
+
+  frc::sim::FlywheelSim intakeSim{intakePlant, consts::intake::physical::INTAKE_MOTOR};
+
   std::shared_ptr<nt::NetworkTable> nt{
       nt::NetworkTableInstance::GetDefault().GetTable("Intake")};
 };