From: Taran Nathan Date: Tue, 19 May 2026 22:15:35 +0000 (-0700) Subject: add manual turret control X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=3456ad6219b5d1b99fff8691ab4939566333bec7;p=FRC2026.git add manual turret control --- diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1018db1..2867bc3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -129,7 +129,7 @@ public class RobotContainer { case Vertigo: // AKA "French Toast" drive = new Drivetrain(vision, new GyroIOPigeon2()); driver = new PS5ControllerDriverConfig(drive, shooter, turret, hood, intake, spindexer); - operator = new Operator(drive); + operator = new Operator(drive, turret, hood, shooter, spindexer); // Detected objects need access to the drivetrain DetectedObject.setDrive(drive); diff --git a/src/main/java/frc/robot/commands/gpm/ManualShoot.java b/src/main/java/frc/robot/commands/gpm/ManualShoot.java new file mode 100644 index 0000000..3842a53 --- /dev/null +++ b/src/main/java/frc/robot/commands/gpm/ManualShoot.java @@ -0,0 +1,341 @@ +package frc.robot.commands.gpm; + +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.LinearFilter; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.Constants; +import frc.robot.constants.FieldConstants; +import frc.robot.constants.ShotInterpolation; +import frc.robot.constants.ShuttleInterpolation; +import frc.robot.controls.Operator; +import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.subsystems.hood.Hood; +import frc.robot.subsystems.hood.HoodConstants; +import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.spindexer.Spindexer; +import frc.robot.subsystems.turret.Turret; +import frc.robot.subsystems.turret.TurretConstants; +import frc.robot.util.PhaseManager; +import frc.robot.util.ShooterPhysics; +import frc.robot.util.ShooterPhysics.TurretState; + +public class ManualShoot extends Command { + private Operator operator; + private Turret turret; + private Drivetrain drivetrain; + private Hood hood; + private Shooter shooter; + private Spindexer spindexer; + + private double turretSetpoint; + private double hoodSetpoint; + + private Pose2d drivepose; + + private final LinearFilter turretAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME)); + private final LinearFilter hoodAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME)); + + private Rotation2d lastTurretAngle; + private Rotation2d turretAngle; + private double turretVelocity; + + private double lastHoodAngle; + private double hoodAngle; + private double hoodVelocity; + + private TurretState goalState; + + private LoggedNetworkNumber phaseDelay = new LoggedNetworkNumber("/Tuning/OPERATOR/Phase Delay", 0.03); // Extrapolation + // delay due + // to latency + + private Translation2d target = FieldConstants.HUB_BLUE.toTranslation2d(); + + private PhaseManager phaseManager = new PhaseManager(); + + private LoggedNetworkNumber hoodOffset = new LoggedNetworkNumber("/Tuning/OPERATOR/Hood Offset", 0.0); + + private LoggedNetworkNumber turretOffset = new LoggedNetworkNumber("/Tuning/OPERATOR/Turret Offet", 0.0); + + private double distanceFromTarget = 0.0; + private LoggedNetworkNumber shuttlingTOFMultiplier = new LoggedNetworkNumber( + "/Tuning/OPERATOR/Shuttling TOF Multiplier", 0.8); + + // private double TOFAdjustment = 0.85; + // private double TOFAdjustment = 1.1; + private LoggedNetworkNumber TOFAdjustment = new LoggedNetworkNumber("/Tuning/OPERATOR/TOF Adjustment", 1.1); + + public ManualShoot(Turret turret, Drivetrain drivetrain, Hood hood, Shooter shooter, Spindexer spindexer) { + this.turret = turret; + this.drivetrain = drivetrain; + this.hood = hood; + this.shooter = shooter; + this.spindexer = spindexer; + drivepose = drivetrain.getPose(); + + goalState = ShooterPhysics.getShotParams( + Translation2d.kZero, + FieldConstants.getHubTranslation().minus(new Translation3d(drivepose.getTranslation())), + 8.0); // Random initial goalState to prevent it being null + + SmartDashboard.putNumber("Manual Shooting Power Scale", 1.0); + + addRequirements(turret, shooter); + } + + public void updateSetpoints(Pose2d drivepose) { + Pose2d turretPosition = drivepose.transformBy( + new Transform2d(TurretConstants.DISTANCE_FROM_ROBOT_CENTER.toTranslation2d(), Rotation2d.kZero)); + + // If the robot is moving, adjust the target position based on velocity + ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds(); + ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw()); + + // Rotational adjustment is not being used, since turret is in center of robot + double turretVelocityX = fieldRelVel.vxMetersPerSecond; + double turretVelocityY = fieldRelVel.vyMetersPerSecond; + + double timeOfFlight = 0; + Pose2d lookaheadPose = turretPosition; + + /* + * Loop (max 20) until lookaheadPose converges BECAUSE --> + * If you're 8m away (t = 1.0s) and moving at 2m/s towards target, you calculate + * for 6m (t = 0.8s) + * At 6m, we run assuming t = 0.8 but then the 6m isn't correct since it was + * derived using t = 1.0s + * So we make a bunch of guesses until it converges + * Early exit when change < 1mm to avoid unnecessary iterations + */ + boolean shuttling = target.equals(FieldConstants.getHubTranslation().toTranslation2d()); + for (int i = 0; i < 20; i++) { + Translation3d lookahead3d = new Translation3d(lookaheadPose.getX(), lookaheadPose.getY(), + TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getZ()); + + // Translation3d target3d = new Translation3d(target.getX(), target.getY(), + // shuttling ? + // FieldConstants.getHubTranslation().getZ() : 0.0); // Height of 0 if it's not + // the hub + + Translation2d x = drivepose.getTranslation().plus( + new Translation2d(0, 1).rotateBy(operator.getLeftRotation()) + ).times(operator.getLeftTrigger() * SmartDashboard.getNumber("Manual Shooting Power Scale", 1.0)); + + Translation3d target3d = new Translation3d(x); + + goalState = ShooterPhysics.getShotParams( + Translation2d.kZero, + target3d.minus(lookahead3d), + 2.0); + + if (!shuttling) { + timeOfFlight = goalState.timeOfFlight() * TOFAdjustment.get(); + } else { + double distance = target.getDistance(lookaheadPose.getTranslation()); + timeOfFlight = distance * shuttlingTOFMultiplier.get(); + } + + double offsetX = turretVelocityX * timeOfFlight; + double offsetY = turretVelocityY * timeOfFlight; + Pose2d newLookaheadPose = new Pose2d( + turretPosition.getTranslation().plus(new Translation2d(offsetX, offsetY)), + turretPosition.getRotation()); + + // early exit if converged (change < 1mm) + if (i > 0 && lookaheadPose.getTranslation().getDistance(newLookaheadPose.getTranslation()) < 0.001) { + lookaheadPose = newLookaheadPose; + break; + } + lookaheadPose = newLookaheadPose; + } + + // Get the field angle relative to the target pose + turretAngle = target.minus(lookaheadPose.getTranslation()).getAngle(); + if (lastTurretAngle == null) { + lastTurretAngle = turretAngle; + } + + // Take the filtered average as the turret's velocity when robot is moving + // translationally + turretVelocity = turretAngleFilter.calculate(turretAngle.minus(lastTurretAngle).getRadians() / Constants.LOOP_TIME); + + lastTurretAngle = turretAngle; + + if (!Constants.DISABLE_LOGGING) { + Logger.recordOutput("Turret/Target Pose", target); + Logger.recordOutput("Lookahead Pose", lookaheadPose); + } + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putNumber("Time of flight", timeOfFlight); + SmartDashboard.putNumber("Turret X-Velocity", turretVelocityX); + SmartDashboard.putNumber("Turret Y-Velocity", turretVelocityY); + } + + // Subtract the rotational angle of the robot from the setpoint + double adjustedTurretSetpoint = MathUtil + .angleModulus(turretAngle.getRadians() - drivepose.getRotation().getRadians()); + + // Shortest path + double error = MathUtil.inputModulus( + Units.radiansToDegrees(adjustedTurretSetpoint) - Units.radiansToDegrees(turret.getPositionRad()), -180, 180); + double potentialSetpoint = Units.radiansToDegrees(turret.getPositionRad()) + error + turretOffset.get(); + + // Stay within physical limits -- if shortest path is past max angle, we go long + // way around + if (potentialSetpoint > TurretConstants.MAX_ANGLE) { + potentialSetpoint -= 360; + } else if (potentialSetpoint < TurretConstants.MIN_ANGLE) { + potentialSetpoint += 360; + } + + turretSetpoint = potentialSetpoint; + + // Pitch is in radians + hoodAngle = goalState.pitch(); + hoodSetpoint = MathUtil.clamp(Units.radiansToDegrees(hoodAngle), HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE); + hoodVelocity = hoodAngleFilter.calculate((hoodAngle - lastHoodAngle) / Constants.LOOP_TIME); + lastHoodAngle = hoodAngle; + + distanceFromTarget = target.getDistance(lookaheadPose.getTranslation()); + Logger.recordOutput("Shooting/distanceToTarget", distanceFromTarget); + } + + public void updateDrivePose() { + Pose2d currentPose = drivetrain.getPose(); + + drivepose = new Pose2d( + currentPose.getTranslation(), + // Uncomment this if robot is backwards + currentPose.getRotation()// .plus(new Rotation2d(Math.PI)) + ); + ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds(); + + // Add a phase delay extrapolation component for latency delay + drivepose = drivepose.exp( + new Twist2d( + robotRelVel.vxMetersPerSecond * phaseDelay.get(), + robotRelVel.vyMetersPerSecond * phaseDelay.get(), + robotRelVel.omegaRadiansPerSecond * phaseDelay.get())); + } + + /** + * Stops and stows all subsystems involved in the command + */ + public void stowEverything() { + turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0); + hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE), 0.0); + shooter.setShooter(0.0); + spindexer.noIndexing = true; + } + + public void underLadder() { + spindexer.noIndexing = true; + } + + // shoot higher + public void bumpUpHoodOffset() { + hoodOffset.set(hoodOffset.get() + 1.0); // 1 deg + } + + // shoot lower + public void bumpDownHoodOffset() { + hoodOffset.set(hoodOffset.get() - 1.0); // 1 deg + } + + // aim more left + public void bumpUpTurretOffset() { + turretOffset.set(turretOffset.get() + 2.5); // 2.5 deg + } + + // aim more right + public void bumpDownTurretOffset() { + turretOffset.set(turretOffset.get() - 2.5); // 2.5 deg + } + + @Override + public void execute() { + // Phase manager stuff + phaseManager.update(drivepose, shooter, turret); + target = phaseManager.getTarget(drivepose); + + updateDrivePose(); + updateSetpoints(drivepose); + + if (phaseManager.isIdle()) { + underLadder(); + } else { + if (spindexer.noIndexing) { + spindexer.noIndexing = false; + } + turret.setFieldRelativeTarget(Rotation2d.fromDegrees(turretSetpoint), + turretVelocity - drivetrain.getAngularRate(2)); + + boolean shuttling = !target.equals(FieldConstants.getHubTranslation().toTranslation2d()); // if we're aiming at + // the hub, we're not + // shuttling + + // shuttling will move the hood so its angles very close to max (less arch) + if (shuttling) { + hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShuttleInterpolation.newHoodMap.get(distanceFromTarget)), + hoodVelocity); + } else { + hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.newHoodMap.get(distanceFromTarget)), + hoodVelocity); + } + + // if (FieldConstants.underTrench(x, y)) { + // System.out.println("Hood forced down"); + // } else { + // hood.forceHoodDown(false); + // } + + // different maps for shuttling vs shooting. Less powerful when shuttling. + if (shuttling) { + shooter.setShooter(-ShuttleInterpolation.shooterVelocityMap.get(distanceFromTarget)); + } else { + shooter.setShooter(-ShotInterpolation.shooterVelocityMap.get(distanceFromTarget)); + } + + if (!Constants.DISABLE_LOGGING) { + // record when shuttling + Logger.recordOutput("Shuttling", shuttling); + // record distance for tuning if needed + Logger.recordOutput("Distance From Target", distanceFromTarget); + } + } + + if (!Constants.DISABLE_LOGGING) { + Logger.recordOutput("Turret Calculated Setpoint", turretSetpoint); + Logger.recordOutput("Hood Calculate Setpoint", hoodSetpoint); + Logger.recordOutput("Shooter Calculate Velocity", goalState.exitVel()); + + Logger.recordOutput("DistanceToTarget", target.getDistance(drivepose.getTranslation())); + } + + // for operator + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putString("Phase Manager State", phaseManager.getCurrentState().toString()); + + } else { + phaseDelay.set(0.03); + } + } + + @Override + public void end(boolean interrupted) { + stowEverything(); + } + +} diff --git a/src/main/java/frc/robot/controls/Operator.java b/src/main/java/frc/robot/controls/Operator.java index 2827c05..48e61a6 100644 --- a/src/main/java/frc/robot/controls/Operator.java +++ b/src/main/java/frc/robot/controls/Operator.java @@ -4,43 +4,78 @@ package frc.robot.controls; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.commands.DoNothing; +import frc.robot.commands.gpm.ManualShoot; import frc.robot.constants.Constants; import frc.robot.subsystems.drivetrain.Drivetrain; -import lib.controllers.GameController; +import frc.robot.subsystems.hood.Hood; +import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.spindexer.Spindexer; +import frc.robot.subsystems.turret.Turret; +import lib.controllers.PS5Controller; +import lib.controllers.PS5Controller.PS5Axis; +import lib.controllers.PS5Controller.PS5Button; -/** - * Controls for the operator, which are almost a duplicate of most of the driver's controls +/** + * Controls for the operator, which are almost a duplicate of most of the + * driver's controls */ public class Operator { - private final GameController driver = new GameController(Constants.OPERATOR_JOY); - - private final Drivetrain drive; - - public Operator(Drivetrain drive) { - this.drive = drive; - } - - public void configureControls() { - // Cancel commands, could be removed if the operator doesn't need this button - driver.get(driver.RIGHT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> { - drive.setIsAlign(false); - drive.setDesiredPose(() -> null); - CommandScheduler.getInstance().cancelAll(); - })); - } - - - public Trigger getRightTrigger(){ - return new Trigger(driver.RIGHT_TRIGGER_BUTTON); - } - public Trigger getLeftTrigger(){ - return new Trigger(driver.LEFT_TRIGGER_BUTTON); - } - public GameController getGameController(){ - return driver; - } + private final PS5Controller driver = new PS5Controller(Constants.OPERATOR_JOY); + + private final Drivetrain drive; + private final Turret turret; + private final Hood hood; + private final Shooter shooter; + private final Spindexer spindexer; + + public Operator(Drivetrain drive, Turret turret, Hood hood, Shooter shooter, Spindexer spindexer) { + this.drive = drive; + this.turret = turret; + this.hood = hood; + this.shooter = shooter; + this.spindexer = spindexer; + } + + public void configureControls() { + // Cancel commands, could be removed if the operator doesn't need this button + // driver.get(driver.RIGHT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> { + // drive.setIsAlign(false); + // drive.setDesiredPose(() -> null); + // CommandScheduler.getInstance().cancelAll(); + // })); + + driver.get(PS5Button.CROSS).toggleOnTrue(new ManualShoot(turret, drive, hood, shooter, spindexer)); + } + + public double getLeftXAxis() { + return driver.get(PS5Axis.LEFT_X); + } + + public double getLeftYAxis() { + return driver.get(PS5Axis.LEFT_Y); + } + + public double getRightXAxis() { + return driver.get(PS5Axis.RIGHT_X); + } + + public double getRightYAxis() { + return driver.get(PS5Axis.RIGHT_Y); + } + + public Rotation2d getLeftRotation() { + return new Rotation2d(getLeftXAxis(), getLeftYAxis()); + } + + public double getRightTrigger() { + return driver.get(PS5Axis.RIGHT_TRIGGER); + } + + public double getLeftTrigger() { + return driver.get(PS5Axis.LEFT_TRIGGER); + } + }