diff --git a/src/main/java/org/team2342/frc/Constants.java b/src/main/java/org/team2342/frc/Constants.java index e0d1c3e..6d83b54 100644 --- a/src/main/java/org/team2342/frc/Constants.java +++ b/src/main/java/org/team2342/frc/Constants.java @@ -198,6 +198,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 { @@ -300,9 +320,9 @@ public static final class CANConstants { public static final int HOOD_MOTOR_ID = 15; public static final int HOOD_ENCODER_ID = 16; - public static final int INTAKE_WHEEL_MOTOR_ID = 17; - public static final int INTAKE_PIVOT_MOTOR_ID = 18; - public static final int INTAKE_PIVOT_ENCODER_ID = 19; + public static final int INTAKE_WHEEL_MOTOR_ID = 18; + public static final int INTAKE_PIVOT_MOTOR_ID = 19; + public static final int INTAKE_PIVOT_ENCODER_ID = 20; public static final int INDEXER_WHEEL_ID = 20; public static final int INDEXER_BELT_ID = 21; diff --git a/src/main/java/org/team2342/frc/RobotContainer.java b/src/main/java/org/team2342/frc/RobotContainer.java index f046a58..b8a06d7 100644 --- a/src/main/java/org/team2342/frc/RobotContainer.java +++ b/src/main/java/org/team2342/frc/RobotContainer.java @@ -42,6 +42,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.shooter.Flywheel; import org.team2342.frc.subsystems.shooter.Hood; @@ -64,6 +65,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 Wheels wheels; @Getter private final Flywheel flywheel; @@ -113,16 +115,19 @@ public RobotContainer() { new SmartMotorIOTalonFX( CANConstants.TURRET_ID, TurretConstants.TURRET_CONFIG.withPIDFFConfigs(TurretConstants.PID_CONFIG))); - flywheel = - new Flywheel( - new SmartMotorIOTalonFX( - CANConstants.FLYWHEEL_MOTOR_ID, - ShooterConstants.FLYWHEEL_CONFIG.withPIDFFConfigs( - ShooterConstants.FLYWHEEL_PID_CONFIGS))); - + flywheel = new Flywheel(new SmartMotorIO() {}); + hood = new Hood(new SmartMotorIO() {}); indexer = new Indexer(new DumbMotorIO() {}, new DumbMotorIO() {}); wheels = new Wheels(new DumbMotorIO() {}); - hood = new Hood(new SmartMotorIO() {}); + pivot = + new Pivot( + new SmartMotorIOTalonFX( + CANConstants.INTAKE_PIVOT_MOTOR_ID, + IntakeConstants.PIVOT_MOTOR_CONFIG.withPIDFFConfigs( + IntakeConstants.PIVOT_MOTOR_PID_CONFIGS))); + new SmartMotorIOTalonFX( + CANConstants.TURRET_ID, + TurretConstants.TURRET_CONFIG.withPIDFFConfigs(TurretConstants.PID_CONFIG)); LoggedPowerDistribution.getInstance(CANConstants.PDH_ID, ModuleType.kRev); break; @@ -177,6 +182,7 @@ public RobotContainer() { ShooterConstants.HOOD_SIM, 1)); turret = new Turret(new SmartMotorIO() {}); + pivot = new Pivot(new SmartMotorIO() {}); break; @@ -196,6 +202,7 @@ public RobotContainer() { new VisionIO() {}); indexer = new Indexer(new DumbMotorIO() {}, new DumbMotorIO() {}); wheels = new Wheels(new DumbMotorIO() {}); + pivot = new Pivot(new SmartMotorIO() {}); flywheel = new Flywheel(new SmartMotorIO() {}); hood = new Hood(new SmartMotorIO() {}); turret = new Turret(new SmartMotorIO() {}); 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"); + } +} diff --git a/src/main/java/org/team2342/lib b/src/main/java/org/team2342/lib index ce2bd1b..6c362e3 160000 --- a/src/main/java/org/team2342/lib +++ b/src/main/java/org/team2342/lib @@ -1 +1 @@ -Subproject commit ce2bd1b8fb600eda5913573d090f162321ee3367 +Subproject commit 6c362e39189b227918ae923db2404b37c2f5c8a1