From 360607a2470eb7cbe369418cef4cdf83a9d47cf9 Mon Sep 17 00:00:00 2001 From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com> Date: Tue, 24 Feb 2026 15:07:02 -0800 Subject: [PATCH] ru happy arnav --- .../frc/robot/commands/gpm/ReverseMotors.java | 15 +-- .../robot/commands/gpm/Superstructure.java | 110 ++++++++---------- .../controls/PS5ControllerDriverConfig.java | 2 +- .../frc/robot/subsystems/turret/Turret.java | 2 +- 4 files changed, 52 insertions(+), 77 deletions(-) diff --git a/src/main/java/frc/robot/commands/gpm/ReverseMotors.java b/src/main/java/frc/robot/commands/gpm/ReverseMotors.java index 4cea990..b0865df 100644 --- a/src/main/java/frc/robot/commands/gpm/ReverseMotors.java +++ b/src/main/java/frc/robot/commands/gpm/ReverseMotors.java @@ -9,25 +9,15 @@ import frc.robot.subsystems.spindexer.Spindexer; import lib.controllers.PS5Controller.PS5Button; public class ReverseMotors extends Command { - // Reverse motors (intake, spindexer, shooter: shoot out constant speed) - - // if (intake != null && spindexer != null && shooter != null){ - // driver.get(PS5Button.CIRCLE).onTrue(new InstantCommand(() ->{ - // intake.spinReverse(); - // // reverse spindexer - // shooter. - // })) - // } private Intake intake; private Spindexer spindexer; - private Shooter shooter; - public ReverseMotors(Intake intake, Spindexer spindexer, Shooter shooter){ + public ReverseMotors(Intake intake, Spindexer spindexer){ this.intake = intake; this.spindexer = spindexer; - addRequirements(intake, spindexer, shooter); + addRequirements(intake, spindexer); } @Override @@ -39,7 +29,6 @@ public class ReverseMotors extends Command { public void execute(){ intake.spinReverse(); spindexer.reverseSpindexer(); - shooter.setShooter(ShooterConstants.SHOOTER_VELOCITY); } @Override diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java index 65dc6fd..2a7ca77 100644 --- a/src/main/java/frc/robot/commands/gpm/Superstructure.java +++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java @@ -36,9 +36,8 @@ public class Superstructure extends Command { private Shooter shooter; private Spindexer spindexer; - // TODO: find maximum interpolation - private Constraints shooterConstraints = new Constraints(Units.inchesToMeters(80.0), 67676767, - HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE); + //TODO: find maximum interpolation + private Constraints shooterConstraints = new Constraints(Units.inchesToMeters(80.0), 67676767, HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE); private double turretSetpoint; private double hoodSetpoint; @@ -47,7 +46,7 @@ public class Superstructure extends Command { 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; @@ -70,66 +69,59 @@ public class Superstructure extends Command { this.hood = hood; this.shooter = shooter; this.spindexer = spindexer; - drivepose = drivetrain.getPose(); + drivepose = drivetrain.getPose(); goalState = ShooterPhysics.getShotParams( - FieldConstants.getHubTranslation().minus(new Translation3d(drivepose.getTranslation())), - 8.0); // Random initial goalState to prevent it being null - + FieldConstants.getHubTranslation().minus(new Translation3d(drivepose.getTranslation())), + 8.0); // Random initial goalState to prevent it being null + addRequirements(turret); } public void updateSetpoints(Pose2d drivepose) { - Pose2d turretPosition = drivepose - .transformBy(new Transform2d((new Translation2d(TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX(), - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY())), new Rotation2d())); - + Pose2d turretPosition = drivepose.transformBy(new Transform2d((new Translation2d(TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX(),TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY())), new Rotation2d())); + // 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 turretVelocityX = + fieldRelVel.vxMetersPerSecond + fieldRelVel.omegaRadiansPerSecond - * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY() - * Math.cos(drivepose.getRotation().getRadians()) - - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX() - * Math.sin(drivepose.getRotation().getRadians())); - double turretVelocityY = fieldRelVel.vyMetersPerSecond + * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY() * Math.cos(drivepose.getRotation().getRadians()) + - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX() * Math.sin(drivepose.getRotation().getRadians())); + double turretVelocityY = + fieldRelVel.vyMetersPerSecond + fieldRelVel.omegaRadiansPerSecond - * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX() - * Math.cos(drivepose.getRotation().getRadians()) - - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY() - * Math.sin(drivepose.getRotation().getRadians())); + * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX() * Math.cos(drivepose.getRotation().getRadians()) + - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY() * Math.sin(drivepose.getRotation().getRadians())); double timeOfFlight; Pose2d lookaheadPose = turretPosition; /* * Loop (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 + * 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 */ 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(), - target == FieldConstants.getHubTranslation().toTranslation2d() - ? FieldConstants.getHubTranslation().getZ() - : 0.0); // Height of 0 if it's not the hub + Translation3d lookahead3d = new Translation3d(lookaheadPose.getX(), lookaheadPose.getY(), TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getZ()); + + Translation3d target3d = new Translation3d(target.getX(), target.getY(), + target == FieldConstants.getHubTranslation().toTranslation2d() ? + FieldConstants.getHubTranslation().getZ() : 0.0); // Height of 0 if it's not the hub goalState = ShooterPhysics.getShotParams( - target3d.minus(lookahead3d), - 2.0); + target3d.minus(lookahead3d), + 2.0); timeOfFlight = goalState.timeOfFlight(); double offsetX = turretVelocityX * timeOfFlight; double offsetY = turretVelocityY * timeOfFlight; - lookaheadPose = new Pose2d( + lookaheadPose = + new Pose2d( turretPosition.getTranslation().plus(new Translation2d(offsetX, offsetY)), turretPosition.getRotation()); } @@ -140,30 +132,26 @@ public class Superstructure extends Command { 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); - + // 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; Logger.recordOutput("Lookahead Pose", lookaheadPose); // Subtract the rotational angle of the robot from the setpoint - double adjustedTurretSetpoint = MathUtil - .angleModulus(turretAngle.getRadians() - drivepose.getRotation().getRadians()); + 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 error = MathUtil.inputModulus(Units.radiansToDegrees(adjustedTurretSetpoint) - Units.radiansToDegrees(turret.getPositionRad()), -180, 180); double potentialSetpoint = Units.radiansToDegrees(turret.getPositionRad()) + error; - // Stay within +/- 200 -- if shortest path is past 200, we go long way around + // Stay within +/- 200 -- if shortest path is past 200, we go long way around double turretRange = TurretConstants.MAX_ANGLE - TurretConstants.MIN_ANGLE; - if (potentialSetpoint > turretRange / 2) { + if (potentialSetpoint > turretRange/2) { potentialSetpoint -= 360; - } else if (potentialSetpoint < -turretRange / 2) { + } else if (potentialSetpoint < -turretRange/2) { potentialSetpoint += 360; } @@ -171,35 +159,34 @@ public class Superstructure extends Command { // Pitch is in radians hoodAngle = goalState.pitch(); - hoodSetpoint = MathUtil.clamp(Units.radiansToDegrees(hoodAngle), HoodConstants.MIN_ANGLE, - HoodConstants.MAX_ANGLE); + hoodSetpoint = MathUtil.clamp(Units.radiansToDegrees(hoodAngle), HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE); hoodVelocity = hoodAngleFilter.calculate((hoodAngle - lastHoodAngle) / Constants.LOOP_TIME); lastHoodAngle = hoodAngle; } - public void updateDrivePose() { + 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)) + 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.exp( - new Twist2d( - robotRelVel.vxMetersPerSecond * phaseDelay, - robotRelVel.vyMetersPerSecond * phaseDelay, - robotRelVel.omegaRadiansPerSecond * phaseDelay)); + new Twist2d( + robotRelVel.vxMetersPerSecond * phaseDelay, + robotRelVel.vyMetersPerSecond * phaseDelay, + robotRelVel.omegaRadiansPerSecond * phaseDelay)); } /** * Stops and stows all subsystems involved in the command */ - public void stowEverything() { + public void stowEverything(){ turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0); hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE), 0.0); shooter.setShooter(0.0); @@ -218,8 +205,7 @@ public class Superstructure extends Command { if (phaseManager.isIdle()) { stowEverything(); } else { - turret.setFieldRelativeTarget(Rotation2d.fromDegrees(turretSetpoint), - turretVelocity - drivetrain.getAngularRate(2)); + turret.setFieldRelativeTarget(Rotation2d.fromDegrees(turretSetpoint), turretVelocity - drivetrain.getAngularRate(2)); hood.setFieldRelativeTarget(Rotation2d.fromDegrees(hoodSetpoint), hoodVelocity); shooter.setShooter(ShotInterpolation.exitVelocityMap.get(goalState.exitVel())); diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index 4527288..4afbec2 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -83,7 +83,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { // Reverse motors if (intake != null && spindexer != null && shooter != null){ driver.get(PS5Button.CIRCLE).onTrue(new InstantCommand(()->{ - reverseMotors = new ReverseMotors(intake, spindexer, shooter); + reverseMotors = new ReverseMotors(intake, spindexer); reverseMotors.schedule(); }) ).onFalse(new InstantCommand(()->{ diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 3a9cc27..99e068e 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -143,7 +143,7 @@ public class Turret extends SubsystemBase implements TurretIO{ } /** - * @return If the turret is at setpoint with tolerance of 2 degrees + * @return If the turret is at setpoint with tolerance of 5 degrees */ public boolean atSetpoint() { return Math.abs(goalAngle.getRadians() - getPositionRad()) < Units.degreesToRadians(5.0); -- 2.39.5