diff --git a/src/main/java/org/team2342/frc/Constants.java b/src/main/java/org/team2342/frc/Constants.java index 43995ad..b308fe6 100644 --- a/src/main/java/org/team2342/frc/Constants.java +++ b/src/main/java/org/team2342/frc/Constants.java @@ -6,6 +6,7 @@ package org.team2342.frc; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; @@ -243,6 +244,40 @@ public static final class ShooterConstants { HOOD_SIM_MOTOR); } + public static final class TurretConstants { + public static final Transform3d TURRET_TRANSFORM = + new Transform3d( + new Translation3d( + Units.inchesToMeters(-4.960), + Units.inchesToMeters(5.997), + Units.inchesToMeters(14.823)), + new Rotation3d(Rotation2d.k180deg)); + + public static final double AT_POSITION_THRESHOLD = 0.01; + + public static final double STARTING_ANGLE = Units.degreesToRadians(0); + public static final double MIN_TURRET_ANGLE = Units.degreesToRadians(-90); + public static final double MAX_TURRET_ANGLE = Units.degreesToRadians(90); + + public static final double GEAR_RATIO = (46.0 / 12.0) * (100.0 / 10.0); + + public static final SmartMotorConfig TURRET_CONFIG = + new SmartMotorConfig() + .withControlType(ControlType.PROFILED_POSITION) + .withGearRatio(GEAR_RATIO) + .withIdleMode(IdleMode.BRAKE) + .withProfileConstraintsRad(new TrapezoidProfile.Constraints(Math.PI, Math.PI)) + .withSupplyCurrentLimit(40); + + public static final PIDFFConfigs PID_CONFIG = new PIDFFConfigs().withKP(100).withKI(10); + + public static final DCMotor TURRET_SIM_MOTOR = DCMotor.getKrakenX60(1); + public static final DCMotorSim TURRET_SIM = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(TURRET_SIM_MOTOR, 0.003, 100.0 / 16.0), + TURRET_SIM_MOTOR); + } + public static final class CANConstants { public static final int PIGEON_ID = 13; public static final int[] FL_IDS = {1, 5, 9}; @@ -262,6 +297,8 @@ public static final class CANConstants { public static final int INDEXER_BELT_ID = 21; public static final int INDEXER_FEEDER_ID = 22; + public static final int TURRET_ID = 23; + public static final int PDH_ID = 62; } } diff --git a/src/main/java/org/team2342/frc/RobotContainer.java b/src/main/java/org/team2342/frc/RobotContainer.java index a6995c6..6de7c81 100644 --- a/src/main/java/org/team2342/frc/RobotContainer.java +++ b/src/main/java/org/team2342/frc/RobotContainer.java @@ -29,6 +29,7 @@ import org.team2342.frc.Constants.IndexerConstants; import org.team2342.frc.Constants.IntakeConstants; import org.team2342.frc.Constants.ShooterConstants; +import org.team2342.frc.Constants.TurretConstants; import org.team2342.frc.Constants.VisionConstants; import org.team2342.frc.commands.DriveCommands; import org.team2342.frc.commands.RotationLockedDrive; @@ -44,6 +45,7 @@ import org.team2342.frc.subsystems.intake.Wheels; import org.team2342.frc.subsystems.shooter.Flywheel; import org.team2342.frc.subsystems.shooter.Hood; +import org.team2342.frc.subsystems.shooter.Turret; import org.team2342.frc.subsystems.vision.Vision; import org.team2342.frc.subsystems.vision.VisionIO; import org.team2342.frc.subsystems.vision.VisionIOPhoton; @@ -67,6 +69,7 @@ public class RobotContainer { @Getter private final Wheels wheels; @Getter private final Flywheel flywheel; @Getter private final Hood hood; + @Getter private final Turret turret; @Getter private final Conductor conductor; @@ -105,35 +108,28 @@ public RobotContainer() { new VisionIOPhoton( VisionConstants.SWERVE_CAMERA_PARAMETERS, PoseStrategy.CONSTRAINED_SOLVEPNP, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR)); + turret = + new Turret( + new SmartMotorIOTalonFX( + CANConstants.TURRET_ID, + TurretConstants.TURRET_CONFIG.withPIDFFConfigs(TurretConstants.PID_CONFIG))); + PoseStrategy.CONSTRAINED_SOLVEPNP, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR), new VisionIOPhoton( VisionConstants.SHOOTER_CAMERA_PARAMETERS, PoseStrategy.CONSTRAINED_SOLVEPNP, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR)); - indexer = - new Indexer( - new DumbMotorIOTalonFXFOC( - CANConstants.INDEXER_BELT_ID, IndexerConstants.INDEXER_BELT_CONFIG), - new DumbMotorIOTalonFXFOC( - CANConstants.INDEXER_FEEDER_ID, IndexerConstants.INDEXER_FEEDER_CONFIG)); - - wheels = - new Wheels( - new DumbMotorIOTalonFXFOC( - CANConstants.INTAKE_WHEEL_MOTOR_ID, - IntakeConstants.INTAKE_WHEELS_MOTOR_CONFIG)); flywheel = new Flywheel( new SmartMotorIOTalonFX( CANConstants.FLYWHEEL_MOTOR_ID, ShooterConstants.FLYWHEEL_CONFIG.withPIDFFConfigs( ShooterConstants.FLYWHEEL_PID_CONFIGS))); - hood = - new Hood( - new SmartMotorIOTalonFX( - CANConstants.HOOD_MOTOR_ID, - ShooterConstants.HOOD_MOTOR_CONFIG.withPIDFFConfigs( - ShooterConstants.HOOD_MOTOR_PID_CONFIGS))); + + indexer = new Indexer(new DumbMotorIO() {}, new DumbMotorIO() {}); + wheels = new Wheels(new DumbMotorIO() {}); + hood = new Hood(new SmartMotorIO() {}); LoggedPowerDistribution.getInstance(CANConstants.PDH_ID, ModuleType.kRev); break; @@ -187,6 +183,7 @@ public RobotContainer() { ShooterConstants.HOOD_SIM_MOTOR, ShooterConstants.HOOD_SIM, 1)); + turret = new Turret(new SmartMotorIO() {}); break; @@ -208,6 +205,7 @@ public RobotContainer() { wheels = new Wheels(new DumbMotorIO() {}); flywheel = new Flywheel(new SmartMotorIO() {}); hood = new Hood(new SmartMotorIO() {}); + turret = new Turret(new SmartMotorIO() {}); break; } diff --git a/src/main/java/org/team2342/frc/subsystems/shooter/Turret.java b/src/main/java/org/team2342/frc/subsystems/shooter/Turret.java new file mode 100644 index 0000000..a130571 --- /dev/null +++ b/src/main/java/org/team2342/frc/subsystems/shooter/Turret.java @@ -0,0 +1,97 @@ +// 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.shooter; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +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.TurretConstants; +import org.team2342.lib.logging.ExecutionLogger; +import org.team2342.lib.motors.smart.SmartMotorIO; +import org.team2342.lib.motors.smart.SmartMotorIOInputsAutoLogged; + +public class Turret extends SubsystemBase { + + private final SmartMotorIO turretMotor; + private final SmartMotorIOInputsAutoLogged inputs = new SmartMotorIOInputsAutoLogged(); + private final Alert motorAlert = new Alert("Turret motor is disconnected!", AlertType.kError); + private double goal = TurretConstants.STARTING_ANGLE; + + public Turret(SmartMotorIO turretMotor) { + this.turretMotor = turretMotor; + setName("Shooter/Turret"); + setDefaultCommand(run(() -> turretMotor.runVoltage(0.0))); + } + + @Override + public void periodic() { + turretMotor.updateInputs(inputs); + Logger.processInputs("Shooter/Turret", inputs); + motorAlert.set(!inputs.motorsConnected[0]); + + ExecutionLogger.log("Shooter/Turret"); + } + + public void runPosition(Rotation2d target) { + this.goal = calculateTurretAngle(target); + turretMotor.runPosition(goal); + } + + public Command runPositionCommand(Rotation2d target) { + return run(() -> runPosition(target)).withName("Turret RunPosition"); + } + + public void goToPosition(Rotation2d target) { + this.goal = calculateTurretAngle(target); + turretMotor.runPosition(goal); + } + + public Command goToPositionCommand(Rotation2d target) { + return run(() -> goToPosition(target)) + .until(() -> Math.abs(inputs.positionRad - goal) <= TurretConstants.AT_POSITION_THRESHOLD) + .withName("Turret GoToPosition"); + } + + public Command runVoltage(double voltage) { + return run(() -> turretMotor.runVoltage(voltage)).withName("Turret Voltage"); + } + + public Command stop() { + return runOnce(() -> turretMotor.runVoltage(0.0)); + } + + @AutoLogOutput(key = "Shooter/Turret/Position") + public Rotation2d getTurretPosition() { + return Rotation2d.fromRadians(inputs.positionRad); + } + + public double getTurretVelocity() { + return inputs.velocityRadPerSec; + } + + @AutoLogOutput(key = "Shooter/Turret/Setpoint") + public Rotation2d getTurretSetpoint() { + return new Rotation2d(goal); + } + + public void zeroTurret() { + turretMotor.setPosition(0.0); + } + + private double calculateTurretAngle(Rotation2d angle) { + double calculatedAngle = MathUtil.inputModulus(angle.getRadians(), -Math.PI, Math.PI); + // if (calculatedAngle < 0) calculatedAngle += 2 * Math.PI; + // if (calculatedAngle > Math.PI * 2) calculatedAngle -= 2 * Math.PI; + return MathUtil.clamp( + calculatedAngle, TurretConstants.MIN_TURRET_ANGLE, TurretConstants.MAX_TURRET_ANGLE); + } +} diff --git a/src/main/java/org/team2342/frc/util/FiringSolver.java b/src/main/java/org/team2342/frc/util/FiringSolver.java index fca2fcd..c494e70 100644 --- a/src/main/java/org/team2342/frc/util/FiringSolver.java +++ b/src/main/java/org/team2342/frc/util/FiringSolver.java @@ -9,6 +9,11 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import org.littletonrobotics.junction.Logger; +import org.team2342.frc.Constants.TurretConstants; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; @@ -28,14 +33,6 @@ public class FiringSolver { private FiringSolution lastSolution = null; - public static Transform3d TURRET_TRANSFORM = - new Transform3d( - new Translation3d( - Units.inchesToMeters(-4.960), - Units.inchesToMeters(5.997), - Units.inchesToMeters(14.823)), - new Rotation3d(Rotation2d.k180deg)); - private static final InterpolatingDoubleTreeMap angleMap = new InterpolatingDoubleTreeMap(); private static final InterpolatingDoubleTreeMap speedMap = new InterpolatingDoubleTreeMap(); private static final InterpolatingDoubleTreeMap tofMap = new InterpolatingDoubleTreeMap(); @@ -77,19 +74,20 @@ public FiringSolution calculate(ChassisSpeeds velocity, Pose2d position) { Translation2d hub = AllianceUtils.flipToAlliance(FieldConstants.Hub.topCenterPoint).toTranslation2d(); - Pose2d turretPose = new Pose3d(position).transformBy(TURRET_TRANSFORM).toPose2d(); + Pose2d turretPose = + new Pose3d(position).transformBy(TurretConstants.TURRET_TRANSFORM).toPose2d(); double robotAngle = position.getRotation().getRadians(); double velX = velocity.vxMetersPerSecond + velocity.omegaRadiansPerSecond - * (TURRET_TRANSFORM.getY() * Math.cos(robotAngle) - - TURRET_TRANSFORM.getX() * Math.sin(robotAngle)); + * (TurretConstants.TURRET_TRANSFORM.getY() * Math.cos(robotAngle) + - TurretConstants.TURRET_TRANSFORM.getX() * Math.sin(robotAngle)); double velY = velocity.vyMetersPerSecond + velocity.omegaRadiansPerSecond - * (TURRET_TRANSFORM.getX() * Math.cos(robotAngle) - - TURRET_TRANSFORM.getY() * Math.sin(robotAngle)); + * (TurretConstants.TURRET_TRANSFORM.getX() * Math.cos(robotAngle) + - TurretConstants.TURRET_TRANSFORM.getY() * Math.sin(robotAngle)); double tof; Pose2d predictedPose = turretPose; @@ -106,8 +104,9 @@ public FiringSolution calculate(ChassisSpeeds velocity, Pose2d position) { Logger.recordOutput("FiringSolver/PredictedPose", predictedPose); Logger.recordOutput("FiringSolver/PredictedDistance", predictedDistance); - Rotation2d turretAngle = - hub.minus(predictedPose.getTranslation()).getAngle().plus(Rotation2d.k180deg); + Rotation2d turretAngle = hub.minus(predictedPose.getTranslation()).getAngle(); + turretAngle = turretAngle.minus(position.getRotation()).minus(Rotation2d.k180deg); + double hoodAngle = angleMap.get(predictedDistance); double wheelSpeed = speedMap.get(predictedDistance);