diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 5139582..702c429 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/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() { diff --git a/src/main/cpp/subsystems/IntakeSubsystem.cpp b/src/main/cpp/subsystems/IntakeSubsystem.cpp index 7f83e86..470ac80 100644 --- a/src/main/cpp/subsystems/IntakeSubsystem.cpp +++ b/src/main/cpp/subsystems/IntakeSubsystem.cpp @@ -4,12 +4,12 @@ #include "subsystems/IntakeSubsystem.h" #include +#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(); } \ No newline at end of file diff --git a/src/main/deploy/commit.txt b/src/main/deploy/commit.txt index 48f52b2..526dc7b 100644 --- a/src/main/deploy/commit.txt +++ b/src/main/deploy/commit.txt @@ -1 +1 @@ -f5a5c55 \ No newline at end of file +7857c51 \ No newline at end of file diff --git a/src/main/include/Autos.h b/src/main/include/Autos.h index a88d6cb..b05964b 100644 --- a/src/main/include/Autos.h +++ b/src/main/include/Autos.h @@ -11,11 +11,12 @@ #include #include #include +#include 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()}; }; diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index 940b683..df634d3 100644 --- a/src/main/include/RobotContainer.h +++ b/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}; }; diff --git a/src/main/include/constants/IntakeConstants.h b/src/main/include/constants/IntakeConstants.h index 066ffdf..4849264 100644 --- a/src/main/include/constants/IntakeConstants.h +++ b/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 diff --git a/src/main/include/subsystems/IntakeSubsystem.h b/src/main/include/subsystems/IntakeSubsystem.h index c7ef220..8334650 100644 --- a/src/main/include/subsystems/IntakeSubsystem.h +++ b/src/main/include/subsystems/IntakeSubsystem.h @@ -10,6 +10,9 @@ #include #include #include +#include +#include +#include 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 intakeMotorVoltageSig = intakeMotor.GetMotorVoltage(); + ctre::phoenix6::StatusSignal 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{ nt::NetworkTableInstance::GetDefault().GetTable("Intake")}; };