diff --git a/src/main/java/org/team2342/frc/Constants.java b/src/main/java/org/team2342/frc/Constants.java index 1be8cd1..eaa7646 100644 --- a/src/main/java/org/team2342/frc/Constants.java +++ b/src/main/java/org/team2342/frc/Constants.java @@ -170,6 +170,26 @@ public static final class IntakeConstants { new DCMotorSim( LinearSystemId.createDCMotorSystem(INTAKE_WHEELS_SIM_MOTOR, 0.003, 1), INTAKE_WHEELS_SIM_MOTOR); + + public static final double PIVOT_GEAR_RATIO = 40; + public static final double MIN_ANGLE = 0.1; + public static final double MAX_ANGLE = 2.23; + + public static final PIDFFConfigs PIVOT_MOTOR_PID_CONFIGS = + new PIDFFConfigs().withKP(45).withKI(15).withKD(7); + public static final SmartMotorConfig PIVOT_MOTOR_CONFIG = + new SmartMotorConfig() + .withGearRatio(1.0) + .withControlType(ControlType.PROFILED_POSITION) + .withIdleMode(IdleMode.BRAKE) + .withMotorInverted(false) + .withSupplyCurrentLimit(40.0) + .withFeedbackConfig( + FeedbackConfig.fused( + CANConstants.INTAKE_PIVOT_ENCODER_ID, PIVOT_GEAR_RATIO, 0.173, true)) + .withProfileConstraintsRad( + new TrapezoidProfile.Constraints( + Units.degreesToRadians(1800), Units.degreesToRadians(540))); } public static final class ShooterConstants { diff --git a/src/main/java/org/team2342/frc/RobotContainer.java b/src/main/java/org/team2342/frc/RobotContainer.java index f26779e..a5894a3 100644 --- a/src/main/java/org/team2342/frc/RobotContainer.java +++ b/src/main/java/org/team2342/frc/RobotContainer.java @@ -44,6 +44,7 @@ import org.team2342.frc.subsystems.drive.ModuleIOSim; import org.team2342.frc.subsystems.drive.ModuleIOTalonFX; import org.team2342.frc.subsystems.indexer.Indexer; +import org.team2342.frc.subsystems.intake.Pivot; import org.team2342.frc.subsystems.intake.Wheels; import org.team2342.frc.subsystems.leds.LEDSubsystem; import org.team2342.frc.subsystems.shooter.Flywheel; @@ -71,6 +72,7 @@ public class RobotContainer { @Getter private final Drive drive; @Getter private final Vision vision; + @Getter private final Pivot pivot; @Getter private final Indexer indexer; @Getter private final Kicker kicker; @Getter private final Wheels wheels; @@ -137,6 +139,12 @@ public RobotContainer() { new Indexer( new DumbMotorIOTalonFXFOC( CANConstants.INDEXER_MOTOR_ID, IndexerConstants.INDEXER_MOTOR_CONFIG)); + pivot = + new Pivot( + new SmartMotorIOTalonFX( + CANConstants.INTAKE_PIVOT_MOTOR_ID, + IntakeConstants.PIVOT_MOTOR_CONFIG.withPIDFFConfigs( + IntakeConstants.PIVOT_MOTOR_PID_CONFIGS))); wheels = new Wheels( new DumbMotorIOTalonFX( @@ -190,6 +198,7 @@ public RobotContainer() { 1)); kicker = new Kicker(new DumbMotorIO() {}); turret = new Turret(new SmartMotorIO() {}); + pivot = new Pivot(new SmartMotorIO() {}); conductor = new Conductor(flywheel, turret, drive::getPose, drive::getChassisSpeeds); leds = @@ -213,6 +222,7 @@ public RobotContainer() { new VisionIO() {}); indexer = new Indexer(new DumbMotorIO() {}); wheels = new Wheels(new DumbMotorIO() {}); + pivot = new Pivot(new SmartMotorIO() {}); flywheel = new Flywheel(new SmartMotorIO() {}); turret = new Turret(new SmartMotorIO() {}); kicker = new Kicker(new DumbMotorIO() {}); diff --git a/src/main/java/org/team2342/frc/subsystems/intake/Pivot.java b/src/main/java/org/team2342/frc/subsystems/intake/Pivot.java new file mode 100644 index 0000000..9dbc222 --- /dev/null +++ b/src/main/java/org/team2342/frc/subsystems/intake/Pivot.java @@ -0,0 +1,67 @@ +// Copyright (c) 2026 Team 2342 +// https://github.com/FRCTeamPhoenix +// +// This source code is licensed under the MIT License. +// See the LICENSE file in the root directory of this project. + +package org.team2342.frc.subsystems.intake; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; +import org.team2342.frc.Constants.IntakeConstants; +import org.team2342.lib.logging.ExecutionLogger; +import org.team2342.lib.motors.smart.SmartMotorIO; +import org.team2342.lib.motors.smart.SmartMotorIOInputsAutoLogged; + +public class Pivot extends SubsystemBase { + + private final SmartMotorIO pivotMotor; + private final SmartMotorIOInputsAutoLogged pivotMotorInputs = new SmartMotorIOInputsAutoLogged(); + + @AutoLogOutput(key = "Intake/Pivot/TargetAngle") + private double goal = 2.23; + + private final Alert pivotMotorAlert = new Alert("Pivot motor is disconnected!", AlertType.kError); + + public Pivot(SmartMotorIO pivotMotor) { + this.pivotMotor = pivotMotor; + setName("Intake/Pivot"); + setDefaultCommand(run(() -> pivotMotor.runVoltage(0.0))); + } + + @Override + public void periodic() { + pivotMotor.updateInputs(pivotMotorInputs); + Logger.processInputs("Intake/Pivot", pivotMotorInputs); + pivotMotorAlert.set(!pivotMotorInputs.motorsConnected[0]); + ExecutionLogger.log("Intake/Pivot"); + } + + public void runAngle(double angle) { + goal = MathUtil.clamp(angle, IntakeConstants.MIN_ANGLE, IntakeConstants.MAX_ANGLE); + pivotMotor.runPosition(goal); + } + + public Command goToAngle(double angle) { + return run(() -> runAngle(angle)) + .until(() -> Math.abs(goal - angle) <= 0.01) + .withName("Pivot Go To Angle"); + } + + public Command runVoltage(double voltage) { + return run(() -> pivotMotor.runVoltage(voltage)).withName("Pivot Run Voltage"); + } + + public Command holdAngle(double angle) { + return run(() -> runAngle(angle)).withName("Pivot Hold Angle"); + } + + public Command stop() { + return runOnce(() -> pivotMotor.runVoltage(0.0)).withName("Pivot Stop"); + } +}