From 108105424dfc89447a516c1e79618db0dcb6bc96 Mon Sep 17 00:00:00 2001 From: AnshG55 Date: Sat, 28 Mar 2026 18:58:15 -0500 Subject: [PATCH 1/8] Shuttling in Neutral zone: Hub in the way --- .../robot/commands/Chassis/TeleopDrive.java | 6 +- .../subsystems/CommandSwerveDrivetrain.java | 57 +++++++++++++++++++ 2 files changed, 62 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/Chassis/TeleopDrive.java b/src/main/java/frc/robot/commands/Chassis/TeleopDrive.java index 4fc0e1b..6af5d5e 100644 --- a/src/main/java/frc/robot/commands/Chassis/TeleopDrive.java +++ b/src/main/java/frc/robot/commands/Chassis/TeleopDrive.java @@ -63,7 +63,11 @@ public void initialize() { public void execute() { ChassisSpeeds targetSpeeds = driveTrain.accelLimitVectorDrive(driveTrain.getHIDspeedsMPS(controller)); if(Math.abs(controller.getRightY()) > 0.7) { - targetSpeeds.omegaRadiansPerSecond = driveTrain.getRotationalVelocity(shooter, hubVector, controller); + if(driveTrain.getStatePose().getX() > 182.11 && driveTrain.getStatePose().getX() < 469.11) { //If the robot is in the neutral Zone shuttle + targetSpeeds.omegaRadiansPerSecond = driveTrain.getRotationalVelocityWhileShuttling(); + } else { //Else it must be in the alliance zone so toggle hub + targetSpeeds.omegaRadiansPerSecond = driveTrain.getRotationalVelocity(shooter, hubVector, controller); + } } else { driveTrain.driveLimiter.setMaxAccel(Constants.Swerve.maxAccelerationFromRest); driveTrain.driveLimiter.setNegativeRateLimit(-5); diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index d089e0a..1bbd8e7 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -313,6 +313,36 @@ public Translation2d getTranslationToHub() { } } + public Translation2d getTranslationToShuttle() { + if(DriverStation.getAlliance().isEmpty()) {return new Translation2d(0, 0);} + if(getStatePose().getY() >= 158.84) {//if the robot is above the hub then this is the translation + if(DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) { + Translation2d originToShuttleUpBlue = new Translation2d(Units.inchesToMeters(181.56+39.37),Units.inchesToMeters(158.84+90.95)); + Translation2d blue = getStatePose().getTranslation().minus(originToShuttleUpBlue).unaryMinus(); + return blue; + } else { + Translation2d originToShuttleUpRed = new Translation2d(Units.inchesToMeters(181.56+287-39.37),Units.inchesToMeters(158.84+90.95)); + Translation2d red = getStatePose().getTranslation().minus(originToShuttleUpRed).unaryMinus(); + return red; + } + } else {//If not above, then below so get below translation + if(DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) { + Translation2d originToShuttleDownBlue = new Translation2d(Units.inchesToMeters(181.56+39.37),Units.inchesToMeters(158.84-90.95)); + Translation2d blue = getStatePose().getTranslation().minus(originToShuttleDownBlue).unaryMinus(); + return blue; + } else { + Translation2d originToShuttleDownRed = new Translation2d(Units.inchesToMeters(181.56+287-39.37),Units.inchesToMeters(158.84-90.95)); + Translation2d red = getStatePose().getTranslation().minus(originToShuttleDownRed).unaryMinus(); + return red; + } + } + + } + + public double getDistanceToShuttle() { + return getTranslationToShuttle().getNorm(); + } + public double getDistanceFromHub() { return getTranslationToHub().getNorm(); } @@ -502,6 +532,33 @@ public double getRotationalVelocity(Shooter shooter, Translation2d hubVector, Co return angleInput + angleOutput; } + public double getRotationalVelocityWhileShuttling() { + double robotAngle = getState().Pose.getRotation().getDegrees(); + if(DriverStation.getAlliance().get() == DriverStation.Alliance.Red){ + double targetAngle = 0; + double angleInput = angularPIDController.calculate(robotAngle, targetAngle); + return angleInput; + } else{ + double targetAngle = 180; + double angleInput = angularPIDController.calculate(robotAngle, targetAngle); + return angleInput; + } + } + + public double getRotationalVelocityWhileShuttlingNotHub() { + double robotAngle = getState().Pose.getRotation().getDegrees(); + if(DriverStation.getAlliance().get() == DriverStation.Alliance.Red){ + + double targetAngle = 0; + double angleInput = angularPIDController.calculate(robotAngle, targetAngle); + return angleInput; + } else{ + double targetAngle = 180; + double angleInput = angularPIDController.calculate(robotAngle, targetAngle); + return angleInput; + } + } + public boolean getFacingHub() {return isFacingHub;} public void setFacingHub(boolean value) {isFacingHub = value;} } From b1eec8d55803f56f00c9313d3ddbd11072f9fa14 Mon Sep 17 00:00:00 2001 From: AnshG55 Date: Sat, 28 Mar 2026 20:20:19 -0500 Subject: [PATCH 2/8] Aiming for shuttling done --- .../robot/commands/Chassis/TeleopDrive.java | 14 +++- .../subsystems/CommandSwerveDrivetrain.java | 82 +++++++++++++++++-- 2 files changed, 86 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/commands/Chassis/TeleopDrive.java b/src/main/java/frc/robot/commands/Chassis/TeleopDrive.java index 6af5d5e..5a3f6b9 100644 --- a/src/main/java/frc/robot/commands/Chassis/TeleopDrive.java +++ b/src/main/java/frc/robot/commands/Chassis/TeleopDrive.java @@ -32,6 +32,8 @@ public class TeleopDrive extends Command { private final SwerveRequest.FieldCentric drive; private Translation2d hubVector = new Translation2d(0, 0); + private Translation2d upShuttleVector = new Translation2d(0,0); + private Translation2d downShuttleVector = new Translation2d(0,0); /** Creates a new TeleopDrive. */ public TeleopDrive(CommandSwerveDrivetrain driveTrain, CommandPS5Controller controller, SwerveRequest.FieldCentric drive, @@ -56,6 +58,16 @@ public void initialize() { hubVector = new Translation2d(Units.inchesToMeters(181.56 + 287),Units.inchesToMeters(158.32)); } } + + if(DriverStation.getAlliance().isPresent()) { + if(DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) { + upShuttleVector = new Translation2d(Units.inchesToMeters(181.56+39.37),Units.inchesToMeters(158.84+90.95)); + downShuttleVector = new Translation2d(Units.inchesToMeters(181.56+39.37),Units.inchesToMeters(158.84-90.95)); + } else { + upShuttleVector = new Translation2d(Units.inchesToMeters(181.56+287-39.37),Units.inchesToMeters(158.84+90.95)); + downShuttleVector = new Translation2d(Units.inchesToMeters(181.56+287-39.37),Units.inchesToMeters(158.84-90.95)); + } + } } // Called every time the scheduler runs while the command is scheduled. @@ -64,7 +76,7 @@ public void execute() { ChassisSpeeds targetSpeeds = driveTrain.accelLimitVectorDrive(driveTrain.getHIDspeedsMPS(controller)); if(Math.abs(controller.getRightY()) > 0.7) { if(driveTrain.getStatePose().getX() > 182.11 && driveTrain.getStatePose().getX() < 469.11) { //If the robot is in the neutral Zone shuttle - targetSpeeds.omegaRadiansPerSecond = driveTrain.getRotationalVelocityWhileShuttling(); + targetSpeeds.omegaRadiansPerSecond = driveTrain.getRotationalVelocityWhileShuttlingNotHub(shooter, upShuttleVector, downShuttleVector, controller); } else { //Else it must be in the alliance zone so toggle hub targetSpeeds.omegaRadiansPerSecond = driveTrain.getRotationalVelocity(shooter, hubVector, controller); } diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 1bbd8e7..3fb55ef 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -545,17 +545,81 @@ public double getRotationalVelocityWhileShuttling() { } } - public double getRotationalVelocityWhileShuttlingNotHub() { - double robotAngle = getState().Pose.getRotation().getDegrees(); - if(DriverStation.getAlliance().get() == DriverStation.Alliance.Red){ - - double targetAngle = 0; + public double getRotationalVelocityWhileShuttlingNotHub(Shooter shooter, Translation2d upShuttleVector, Translation2d downShuttleVector, CommandPS5Controller controller) { + if(getStatePose().getY() >= 158.84){ + Translation2d robotVector = getState().Pose.getTranslation(); + Translation2d targetVector = upShuttleVector.minus(robotVector); + double targetAngle = targetVector.getAngle().getDegrees(); + double robotAngle = robotVector.getAngle().getDegrees(); + // If shooting correct for movement, otherwise just target shuttlespot regardless of where you are + if (shooter.getIsShooting()) { + targetAngle = getAngleSetpoint(); + driveLimiter.setMaxAccel(3); + driveLimiter.setNegativeRateLimit(-3); + } else { + targetAngle = upShuttleVector.minus(robotVector).getAngle().getDegrees(); + } + // Keep angles in the range (-180, 180) + if(targetAngle - robotAngle > 180) { + targetAngle -= 360; + } else if(targetAngle - robotAngle < -180) { + targetAngle += 360; + } + // Robot angle is within 3 degrees of target angle and rotational velocityy is less than 0.2 rad/s + if(Math.abs(robotAngle - targetAngle) < 3 && getState().Speeds.omegaRadiansPerSecond < 0.2) { + //setFacingHub(true); + } else { + //setFacingHub(false); + } double angleInput = angularPIDController.calculate(robotAngle, targetAngle); - return angleInput; - } else{ - double targetAngle = 180; + + Translation2d robotFieldVel = new Translation2d( + getState().Speeds.vxMetersPerSecond, + getState().Speeds.vyMetersPerSecond + ).rotateBy(getState().Pose.getRotation()); // Field Relative Robot Velocity + // Calculation of unit tangent vector. Take vector to hub, rotate by 90 degrees to get tangential vector, normalize tangential vector + Translation2d unitTangent = targetVector.rotateBy(new Rotation2d(Math.PI/2)).div(targetVector.getNorm()); + // Angle correct the opposite direction of movement using w = -v/R + double angleOutput = -unitTangent.dot(robotFieldVel)/targetVector.getNorm(); + return angleInput+angleOutput; + } if (getStatePose().getY() < 158.84) {//If robot is below the hub on the map + Translation2d robotVector = getState().Pose.getTranslation(); + Translation2d targetVector = downShuttleVector.minus(robotVector); + double targetAngle = targetVector.getAngle().getDegrees(); + double robotAngle = robotVector.getAngle().getDegrees(); + // If shooting correct for movement, otherwise just target shuttlespot regardless of where you are + if (shooter.getIsShooting()) { + targetAngle = getAngleSetpoint(); + driveLimiter.setMaxAccel(3); + driveLimiter.setNegativeRateLimit(-3); + } else { + targetAngle = downShuttleVector.minus(robotVector).getAngle().getDegrees(); + } + // Keep angles in the range (-180, 180) + if(targetAngle - robotAngle > 180) { + targetAngle -= 360; + } else if(targetAngle - robotAngle < -180) { + targetAngle += 360; + } + // Robot angle is within 3 degrees of target angle and rotational velocityy is less than 0.2 rad/s + if(Math.abs(robotAngle - targetAngle) < 3 && getState().Speeds.omegaRadiansPerSecond < 0.2) { + //setFacingHub(true); + } else { + //setFacingHub(false); + } double angleInput = angularPIDController.calculate(robotAngle, targetAngle); - return angleInput; + + Translation2d robotFieldVel = new Translation2d( + getState().Speeds.vxMetersPerSecond, + getState().Speeds.vyMetersPerSecond + ).rotateBy(getState().Pose.getRotation()); // Field Relative Robot Velocity + // Calculation of unit tangent vector. Take vector to hub, rotate by 90 degrees to get tangential vector, normalize tangential vector + Translation2d unitTangent = targetVector.rotateBy(new Rotation2d(Math.PI/2)).div(targetVector.getNorm()); + // Angle correct the opposite direction of movement using w = -v/R + double angleOutput = -unitTangent.dot(robotFieldVel)/targetVector.getNorm(); + return angleInput+angleOutput; + } else { //If robot is out of field + return 0; } } From 7a68ebd8a4b849100e38a93de82f795baffa2f4e Mon Sep 17 00:00:00 2001 From: AnshG55 Date: Sat, 28 Mar 2026 21:02:44 -0500 Subject: [PATCH 3/8] Just need to use conversion --- .../subsystems/CommandSwerveDrivetrain.java | 16 ++++++++++++++++ src/main/java/frc/robot/subsystems/Shooter.java | 7 +++++-- 2 files changed, 21 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 3fb55ef..a462586 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -339,6 +339,14 @@ public Translation2d getTranslationToShuttle() { } + public Translation2d getTranslationForAutoAim() { + if(getStatePose().getX() > 182.11 && getStatePose().getX() < 469.11) { + return getTranslationToShuttle(); + } else { + return getTranslationToHub(); + } + } + public double getDistanceToShuttle() { return getTranslationToShuttle().getNorm(); } @@ -347,6 +355,14 @@ public double getDistanceFromHub() { return getTranslationToHub().getNorm(); } + public double getDistanceForAutoAim() { + if(getStatePose().getX() > 182.11 && getStatePose().getX() < 469.11) { + return getDistanceToShuttle(); + } else { + return getDistanceFromHub(); + } +} + /** * Constructs a CTRE SwerveDrivetrain using the specified constants. diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 455d831..7065b0a 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -66,6 +66,7 @@ public class Shooter extends SubsystemBase { private final double targetVelocityRotations = Units.radiansToRotations(targetVelocityMetersPerSec/Units.inchesToMeters(2)); private double speed = 0.7; + //SysID private final VoltageOut m_voltReq = new VoltageOut(0.0); @@ -261,13 +262,15 @@ public double getInterPolVel(double distance) { return Math.sqrt(velmps); } + // Interpolation Request for Velocity public double interpolTargetSpeed(CommandSwerveDrivetrain driveTrain, ShooterHood shooterHood) { - double distance = driveTrain.getDistanceFromHub(); + + double distance = driveTrain.getDistanceForAutoAim(); double velmps = tableVel.get(distance); // 1. Get the direction to the hub (Must point AT the hub) - Translation2d toHubDir = driveTrain.getTranslationToHub(); + Translation2d toHubDir = driveTrain.getTranslationForAutoAim(); // 2. Calculate the "Static" components (what the ball does if robot is still) double hoodAngleRads = Math.toRadians(81 - (360 * shooterHood.getTableAutoAimValue(distance))); From d08d90487cccf2eaf2d5a3a07b3fc83077485e1d Mon Sep 17 00:00:00 2001 From: AnshG55 Date: Sat, 28 Mar 2026 21:09:50 -0500 Subject: [PATCH 4/8] Should be ready for testing --- .../java/frc/robot/commands/Chassis/TeleopDrive.java | 2 +- .../frc/robot/subsystems/CommandSwerveDrivetrain.java | 11 ++++++----- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/commands/Chassis/TeleopDrive.java b/src/main/java/frc/robot/commands/Chassis/TeleopDrive.java index 5a3f6b9..2664837 100644 --- a/src/main/java/frc/robot/commands/Chassis/TeleopDrive.java +++ b/src/main/java/frc/robot/commands/Chassis/TeleopDrive.java @@ -75,7 +75,7 @@ public void initialize() { public void execute() { ChassisSpeeds targetSpeeds = driveTrain.accelLimitVectorDrive(driveTrain.getHIDspeedsMPS(controller)); if(Math.abs(controller.getRightY()) > 0.7) { - if(driveTrain.getStatePose().getX() > 182.11 && driveTrain.getStatePose().getX() < 469.11) { //If the robot is in the neutral Zone shuttle + if(driveTrain.getStatePose().getX() > Units.inchesToMeters(182.11) && driveTrain.getStatePose().getX() < Units.inchesToMeters(469.11)) { //If the robot is in the neutral Zone shuttle targetSpeeds.omegaRadiansPerSecond = driveTrain.getRotationalVelocityWhileShuttlingNotHub(shooter, upShuttleVector, downShuttleVector, controller); } else { //Else it must be in the alliance zone so toggle hub targetSpeeds.omegaRadiansPerSecond = driveTrain.getRotationalVelocity(shooter, hubVector, controller); diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index a462586..6e69b28 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -315,7 +315,7 @@ public Translation2d getTranslationToHub() { public Translation2d getTranslationToShuttle() { if(DriverStation.getAlliance().isEmpty()) {return new Translation2d(0, 0);} - if(getStatePose().getY() >= 158.84) {//if the robot is above the hub then this is the translation + if(getStatePose().getY() >= Units.inchesToMeters(158.84)) {//if the robot is above the hub then this is the translation if(DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) { Translation2d originToShuttleUpBlue = new Translation2d(Units.inchesToMeters(181.56+39.37),Units.inchesToMeters(158.84+90.95)); Translation2d blue = getStatePose().getTranslation().minus(originToShuttleUpBlue).unaryMinus(); @@ -340,7 +340,7 @@ public Translation2d getTranslationToShuttle() { } public Translation2d getTranslationForAutoAim() { - if(getStatePose().getX() > 182.11 && getStatePose().getX() < 469.11) { + if(getStatePose().getX() > Units.inchesToMeters(182.11) && getStatePose().getX() < Units.inchesToMeters(469.11)) { return getTranslationToShuttle(); } else { return getTranslationToHub(); @@ -356,7 +356,7 @@ public double getDistanceFromHub() { } public double getDistanceForAutoAim() { - if(getStatePose().getX() > 182.11 && getStatePose().getX() < 469.11) { + if(getStatePose().getX() > Units.inchesToMeters(182.11) && getStatePose().getX() < Units.inchesToMeters(469.11)) { return getDistanceToShuttle(); } else { return getDistanceFromHub(); @@ -548,6 +548,7 @@ public double getRotationalVelocity(Shooter shooter, Translation2d hubVector, Co return angleInput + angleOutput; } + //Old shuttling command, not needed so can be deleted if Umar really wants. public double getRotationalVelocityWhileShuttling() { double robotAngle = getState().Pose.getRotation().getDegrees(); if(DriverStation.getAlliance().get() == DriverStation.Alliance.Red){ @@ -562,7 +563,7 @@ public double getRotationalVelocityWhileShuttling() { } public double getRotationalVelocityWhileShuttlingNotHub(Shooter shooter, Translation2d upShuttleVector, Translation2d downShuttleVector, CommandPS5Controller controller) { - if(getStatePose().getY() >= 158.84){ + if(getStatePose().getY() >= Units.inchesToMeters(158.84)){ Translation2d robotVector = getState().Pose.getTranslation(); Translation2d targetVector = upShuttleVector.minus(robotVector); double targetAngle = targetVector.getAngle().getDegrees(); @@ -598,7 +599,7 @@ public double getRotationalVelocityWhileShuttlingNotHub(Shooter shooter, Transla // Angle correct the opposite direction of movement using w = -v/R double angleOutput = -unitTangent.dot(robotFieldVel)/targetVector.getNorm(); return angleInput+angleOutput; - } if (getStatePose().getY() < 158.84) {//If robot is below the hub on the map + } if (getStatePose().getY() < Units.inchesToMeters(158.84)) {//If robot is below the hub on the map Translation2d robotVector = getState().Pose.getTranslation(); Translation2d targetVector = downShuttleVector.minus(robotVector); double targetAngle = targetVector.getAngle().getDegrees(); From e8d6c8b493395dfe4d937c4abb44071b7a28ac33 Mon Sep 17 00:00:00 2001 From: AnshG55 Date: Sun, 29 Mar 2026 19:10:57 -0500 Subject: [PATCH 5/8] Looks better :) --- .../commands/ShooterHood/PID/AutoAim.java | 2 +- .../subsystems/CommandSwerveDrivetrain.java | 173 +++++------------- .../java/frc/robot/subsystems/Shooter.java | 2 +- 3 files changed, 46 insertions(+), 131 deletions(-) diff --git a/src/main/java/frc/robot/commands/ShooterHood/PID/AutoAim.java b/src/main/java/frc/robot/commands/ShooterHood/PID/AutoAim.java index 7267565..857773e 100644 --- a/src/main/java/frc/robot/commands/ShooterHood/PID/AutoAim.java +++ b/src/main/java/frc/robot/commands/ShooterHood/PID/AutoAim.java @@ -29,7 +29,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - shooterHood.autoAim(drivetrain.getDistanceFromHub()); + shooterHood.autoAim(drivetrain.getDistanceForAutoAim()); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 6e69b28..a930a82 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -347,19 +347,11 @@ public Translation2d getTranslationForAutoAim() { } } - public double getDistanceToShuttle() { - return getTranslationToShuttle().getNorm(); - } - - public double getDistanceFromHub() { - return getTranslationToHub().getNorm(); - } - public double getDistanceForAutoAim() { if(getStatePose().getX() > Units.inchesToMeters(182.11) && getStatePose().getX() < Units.inchesToMeters(469.11)) { - return getDistanceToShuttle(); + return getTranslationToShuttle().getNorm(); } else { - return getDistanceFromHub(); + return getTranslationToHub().getNorm(); } } @@ -509,132 +501,55 @@ public ChassisSpeeds getSpeeds() { return this.getState().Speeds; } - public double getRotationalVelocity(Shooter shooter, Translation2d hubVector, CommandPS5Controller controller) { - Translation2d robotVector = getState().Pose.getTranslation(); - Translation2d targetVector = hubVector.minus(robotVector); - double targetAngle = targetVector.getAngle().getDegrees(); - double robotAngle = getState().Pose.getRotation().getDegrees(); - // If shooting correct for movement, otherwise just target hub regardless of where you are - if (shooter.getIsShooting()) { - targetAngle = getAngleSetpoint(); - driveLimiter.setMaxAccel(3); - driveLimiter.setNegativeRateLimit(-3); - } else { - targetAngle = hubVector.minus(robotVector).getAngle().getDegrees(); - } - // Keep angles in the range (-180, 180) - if(targetAngle - robotAngle > 180) { - targetAngle -= 360; - } else if(targetAngle - robotAngle < -180) { - targetAngle += 360; - } - // Robot angle is within 3 degrees of target angle and rotational velocityy is less than 0.2 rad/s - if(Math.abs(robotAngle - targetAngle) < 3 && getState().Speeds.omegaRadiansPerSecond < 0.2) { - setFacingHub(true); - } else { - setFacingHub(false); - } - double angleInput = angularPIDController.calculate(robotAngle, targetAngle); - - Translation2d robotFieldVel = new Translation2d( - getState().Speeds.vxMetersPerSecond, - getState().Speeds.vyMetersPerSecond - ).rotateBy(getState().Pose.getRotation()); // Field Relative Robot Velocity - // Calculation of unit tangent vector. Take vector to hub, rotate by 90 degrees to get tangential vector, normalize tangential vector - Translation2d unitTangent = targetVector.rotateBy(new Rotation2d(Math.PI/2)).div(targetVector.getNorm()); - // Angle correct the opposite direction of movement using w = -v/R - double angleOutput = -unitTangent.dot(robotFieldVel)/targetVector.getNorm(); - - return angleInput + angleOutput; - } - - //Old shuttling command, not needed so can be deleted if Umar really wants. - public double getRotationalVelocityWhileShuttling() { + public double getRotationalVelocity(Shooter shooter, Translation2d targetSpotVector, CommandPS5Controller controller) { + Translation2d robotVector = getState().Pose.getTranslation(); + Translation2d targetVector = targetSpotVector.minus(robotVector); + double targetAngle = targetVector.getAngle().getDegrees(); double robotAngle = getState().Pose.getRotation().getDegrees(); - if(DriverStation.getAlliance().get() == DriverStation.Alliance.Red){ - double targetAngle = 0; - double angleInput = angularPIDController.calculate(robotAngle, targetAngle); - return angleInput; - } else{ - double targetAngle = 180; - double angleInput = angularPIDController.calculate(robotAngle, targetAngle); - return angleInput; + // If shooting correct for movement, otherwise just target hub regardless of where you are + if (shooter.getIsShooting()) { + targetAngle = getAngleSetpoint(); + driveLimiter.setMaxAccel(3); + driveLimiter.setNegativeRateLimit(-3); + } else { + targetAngle = targetSpotVector.minus(robotVector).getAngle().getDegrees(); } + // Keep angles in the range (-180, 180) + if(targetAngle - robotAngle > 180) { + targetAngle -= 360; + } else if(targetAngle - robotAngle < -180) { + targetAngle += 360; + } + // Robot angle is within 3 degrees of target angle and rotational velocityy is less than 0.2 rad/s + if(Math.abs(robotAngle - targetAngle) < 3 && getState().Speeds.omegaRadiansPerSecond < 0.2) { + setFacingHub(true); + } else { + setFacingHub(false); + } + double angleInput = angularPIDController.calculate(robotAngle, targetAngle); + + Translation2d robotFieldVel = new Translation2d( + getState().Speeds.vxMetersPerSecond, + getState().Speeds.vyMetersPerSecond + ).rotateBy(getState().Pose.getRotation()); // Field Relative Robot Velocity + // Calculation of unit tangent vector. Take vector to hub, rotate by 90 degrees to get tangential vector, normalize tangential vector + Translation2d unitTangent = targetVector.rotateBy(new Rotation2d(Math.PI/2)).div(targetVector.getNorm()); + // Angle correct the opposite direction of movement using w = -v/R + double angleOutput = -unitTangent.dot(robotFieldVel)/targetVector.getNorm(); + + return angleInput + angleOutput; + } + + public double getRotationalVelocityHub(Shooter shooter, Translation2d hubVector, CommandPS5Controller controller) { + return getRotationalVelocity(shooter, hubVector, controller); } public double getRotationalVelocityWhileShuttlingNotHub(Shooter shooter, Translation2d upShuttleVector, Translation2d downShuttleVector, CommandPS5Controller controller) { if(getStatePose().getY() >= Units.inchesToMeters(158.84)){ - Translation2d robotVector = getState().Pose.getTranslation(); - Translation2d targetVector = upShuttleVector.minus(robotVector); - double targetAngle = targetVector.getAngle().getDegrees(); - double robotAngle = robotVector.getAngle().getDegrees(); - // If shooting correct for movement, otherwise just target shuttlespot regardless of where you are - if (shooter.getIsShooting()) { - targetAngle = getAngleSetpoint(); - driveLimiter.setMaxAccel(3); - driveLimiter.setNegativeRateLimit(-3); - } else { - targetAngle = upShuttleVector.minus(robotVector).getAngle().getDegrees(); - } - // Keep angles in the range (-180, 180) - if(targetAngle - robotAngle > 180) { - targetAngle -= 360; - } else if(targetAngle - robotAngle < -180) { - targetAngle += 360; - } - // Robot angle is within 3 degrees of target angle and rotational velocityy is less than 0.2 rad/s - if(Math.abs(robotAngle - targetAngle) < 3 && getState().Speeds.omegaRadiansPerSecond < 0.2) { - //setFacingHub(true); - } else { - //setFacingHub(false); - } - double angleInput = angularPIDController.calculate(robotAngle, targetAngle); - - Translation2d robotFieldVel = new Translation2d( - getState().Speeds.vxMetersPerSecond, - getState().Speeds.vyMetersPerSecond - ).rotateBy(getState().Pose.getRotation()); // Field Relative Robot Velocity - // Calculation of unit tangent vector. Take vector to hub, rotate by 90 degrees to get tangential vector, normalize tangential vector - Translation2d unitTangent = targetVector.rotateBy(new Rotation2d(Math.PI/2)).div(targetVector.getNorm()); - // Angle correct the opposite direction of movement using w = -v/R - double angleOutput = -unitTangent.dot(robotFieldVel)/targetVector.getNorm(); - return angleInput+angleOutput; + return getRotationalVelocity(shooter, upShuttleVector, controller); + } if (getStatePose().getY() < Units.inchesToMeters(158.84)) {//If robot is below the hub on the map - Translation2d robotVector = getState().Pose.getTranslation(); - Translation2d targetVector = downShuttleVector.minus(robotVector); - double targetAngle = targetVector.getAngle().getDegrees(); - double robotAngle = robotVector.getAngle().getDegrees(); - // If shooting correct for movement, otherwise just target shuttlespot regardless of where you are - if (shooter.getIsShooting()) { - targetAngle = getAngleSetpoint(); - driveLimiter.setMaxAccel(3); - driveLimiter.setNegativeRateLimit(-3); - } else { - targetAngle = downShuttleVector.minus(robotVector).getAngle().getDegrees(); - } - // Keep angles in the range (-180, 180) - if(targetAngle - robotAngle > 180) { - targetAngle -= 360; - } else if(targetAngle - robotAngle < -180) { - targetAngle += 360; - } - // Robot angle is within 3 degrees of target angle and rotational velocityy is less than 0.2 rad/s - if(Math.abs(robotAngle - targetAngle) < 3 && getState().Speeds.omegaRadiansPerSecond < 0.2) { - //setFacingHub(true); - } else { - //setFacingHub(false); - } - double angleInput = angularPIDController.calculate(robotAngle, targetAngle); - - Translation2d robotFieldVel = new Translation2d( - getState().Speeds.vxMetersPerSecond, - getState().Speeds.vyMetersPerSecond - ).rotateBy(getState().Pose.getRotation()); // Field Relative Robot Velocity - // Calculation of unit tangent vector. Take vector to hub, rotate by 90 degrees to get tangential vector, normalize tangential vector - Translation2d unitTangent = targetVector.rotateBy(new Rotation2d(Math.PI/2)).div(targetVector.getNorm()); - // Angle correct the opposite direction of movement using w = -v/R - double angleOutput = -unitTangent.dot(robotFieldVel)/targetVector.getNorm(); - return angleInput+angleOutput; + return getRotationalVelocity(shooter, downShuttleVector, controller); } else { //If robot is out of field return 0; } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 7065b0a..b1b9897 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -160,7 +160,7 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { public void revAtVelocity(double velocityMetersPerSec, CommandSwerveDrivetrain driveTrain, ShooterHood shooterHood) { Translation2d ballVelocityVector = driveTrain.getTranslationToHub(); ballVelocityVector = ballVelocityVector.times(0.45 * velocityMetersPerSec/ballVelocityVector.getNorm()); - ballVelocityVector = ballVelocityVector.times(Math.cos(Math.toRadians(360 * shooterHood.getTableAutoAimValue(driveTrain.getDistanceFromHub()) + 9))); + ballVelocityVector = ballVelocityVector.times(Math.cos(Math.toRadians(360 * shooterHood.getTableAutoAimValue(driveTrain.getDistanceForAutoAim()) + 9))); Translation2d robotVelocityVector = new Translation2d(driveTrain.getRobotRelativeSpeeds().vxMetersPerSecond, driveTrain.getRobotRelativeSpeeds().vyMetersPerSecond); Translation2d velocityVector = ballVelocityVector.minus(robotVelocityVector); double newVelocityMetersPerSec = velocityVector.getNorm(); From 5b1c1be61db3b76476cab73e42e36af1fa52b509 Mon Sep 17 00:00:00 2001 From: AnshG55 Date: Sun, 29 Mar 2026 22:47:34 -0500 Subject: [PATCH 6/8] Sim Tested --- src/main/java/frc/robot/commands/Chassis/TeleopDrive.java | 8 ++++---- .../frc/robot/subsystems/CommandSwerveDrivetrain.java | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/commands/Chassis/TeleopDrive.java b/src/main/java/frc/robot/commands/Chassis/TeleopDrive.java index 2664837..3918d12 100644 --- a/src/main/java/frc/robot/commands/Chassis/TeleopDrive.java +++ b/src/main/java/frc/robot/commands/Chassis/TeleopDrive.java @@ -61,11 +61,11 @@ public void initialize() { if(DriverStation.getAlliance().isPresent()) { if(DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) { - upShuttleVector = new Translation2d(Units.inchesToMeters(181.56+39.37),Units.inchesToMeters(158.84+90.95)); - downShuttleVector = new Translation2d(Units.inchesToMeters(181.56+39.37),Units.inchesToMeters(158.84-90.95)); + upShuttleVector = new Translation2d(Units.inchesToMeters(181.56-39.37),Units.inchesToMeters(158.84+90.95)); + downShuttleVector = new Translation2d(Units.inchesToMeters(181.56-39.37),Units.inchesToMeters(158.84-90.95)); } else { - upShuttleVector = new Translation2d(Units.inchesToMeters(181.56+287-39.37),Units.inchesToMeters(158.84+90.95)); - downShuttleVector = new Translation2d(Units.inchesToMeters(181.56+287-39.37),Units.inchesToMeters(158.84-90.95)); + upShuttleVector = new Translation2d(Units.inchesToMeters(181.56+287+39.37),Units.inchesToMeters(158.84+90.95)); + downShuttleVector = new Translation2d(Units.inchesToMeters(181.56+287+39.37),Units.inchesToMeters(158.84-90.95)); } } } diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index a930a82..3bc1bb2 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -317,21 +317,21 @@ public Translation2d getTranslationToShuttle() { if(DriverStation.getAlliance().isEmpty()) {return new Translation2d(0, 0);} if(getStatePose().getY() >= Units.inchesToMeters(158.84)) {//if the robot is above the hub then this is the translation if(DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) { - Translation2d originToShuttleUpBlue = new Translation2d(Units.inchesToMeters(181.56+39.37),Units.inchesToMeters(158.84+90.95)); + Translation2d originToShuttleUpBlue = new Translation2d(Units.inchesToMeters(181.56-39.37),Units.inchesToMeters(158.84+90.95)); Translation2d blue = getStatePose().getTranslation().minus(originToShuttleUpBlue).unaryMinus(); return blue; } else { - Translation2d originToShuttleUpRed = new Translation2d(Units.inchesToMeters(181.56+287-39.37),Units.inchesToMeters(158.84+90.95)); + Translation2d originToShuttleUpRed = new Translation2d(Units.inchesToMeters(181.56+287+39.37),Units.inchesToMeters(158.84+90.95)); Translation2d red = getStatePose().getTranslation().minus(originToShuttleUpRed).unaryMinus(); return red; } } else {//If not above, then below so get below translation if(DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) { - Translation2d originToShuttleDownBlue = new Translation2d(Units.inchesToMeters(181.56+39.37),Units.inchesToMeters(158.84-90.95)); + Translation2d originToShuttleDownBlue = new Translation2d(Units.inchesToMeters(181.56-39.37),Units.inchesToMeters(158.84-90.95)); Translation2d blue = getStatePose().getTranslation().minus(originToShuttleDownBlue).unaryMinus(); return blue; } else { - Translation2d originToShuttleDownRed = new Translation2d(Units.inchesToMeters(181.56+287-39.37),Units.inchesToMeters(158.84-90.95)); + Translation2d originToShuttleDownRed = new Translation2d(Units.inchesToMeters(181.56+287+39.37),Units.inchesToMeters(158.84-90.95)); Translation2d red = getStatePose().getTranslation().minus(originToShuttleDownRed).unaryMinus(); return red; } From b023021872395a3a7c4d515843534c6533508d75 Mon Sep 17 00:00:00 2001 From: AnshG55 Date: Mon, 30 Mar 2026 09:45:05 -0500 Subject: [PATCH 7/8] Sim Tested --- src/main/java/frc/robot/commands/Chassis/TeleopDrive.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/Chassis/TeleopDrive.java b/src/main/java/frc/robot/commands/Chassis/TeleopDrive.java index 3918d12..fd63187 100644 --- a/src/main/java/frc/robot/commands/Chassis/TeleopDrive.java +++ b/src/main/java/frc/robot/commands/Chassis/TeleopDrive.java @@ -74,7 +74,7 @@ public void initialize() { @Override public void execute() { ChassisSpeeds targetSpeeds = driveTrain.accelLimitVectorDrive(driveTrain.getHIDspeedsMPS(controller)); - if(Math.abs(controller.getRightY()) > 0.7) { + if(Math.abs(controller.getRightY()) > 0.7) {//for simunation: || controller.circle().getAsBoolean() if(driveTrain.getStatePose().getX() > Units.inchesToMeters(182.11) && driveTrain.getStatePose().getX() < Units.inchesToMeters(469.11)) { //If the robot is in the neutral Zone shuttle targetSpeeds.omegaRadiansPerSecond = driveTrain.getRotationalVelocityWhileShuttlingNotHub(shooter, upShuttleVector, downShuttleVector, controller); } else { //Else it must be in the alliance zone so toggle hub From 7786a7b8d431507652898044bf510049b6737124 Mon Sep 17 00:00:00 2001 From: AnshG55 Date: Wed, 1 Apr 2026 10:18:50 -0500 Subject: [PATCH 8/8] Max velocity while shuttling is 4.5; Fixed other issues --- src/main/java/frc/robot/RobotContainer.java | 1 - src/main/java/frc/robot/commands/Chassis/TeleopDrive.java | 2 +- src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java | 2 +- 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7c9a5d5..5ad9213 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -166,7 +166,6 @@ private void configureBindings() { new ShooterHoodDown(shooterHood), new AutoAim(shooterHood, driveTrain))); - // Run SysId routines when holding back/start and X/Y. // Note that each routine should be run exactly once in a single log. //operatorController.back().and(operatorController.y()).whileTrue(shooter.sysIdDynamic(SysIdRoutine.Direction.kForward)); diff --git a/src/main/java/frc/robot/commands/Chassis/TeleopDrive.java b/src/main/java/frc/robot/commands/Chassis/TeleopDrive.java index fd63187..375ffa8 100644 --- a/src/main/java/frc/robot/commands/Chassis/TeleopDrive.java +++ b/src/main/java/frc/robot/commands/Chassis/TeleopDrive.java @@ -74,7 +74,7 @@ public void initialize() { @Override public void execute() { ChassisSpeeds targetSpeeds = driveTrain.accelLimitVectorDrive(driveTrain.getHIDspeedsMPS(controller)); - if(Math.abs(controller.getRightY()) > 0.7) {//for simunation: || controller.circle().getAsBoolean() + if(Math.abs(controller.getRightY() ) > 0.7) {//for simunation: || controller.circle().getAsBoolean() if(driveTrain.getStatePose().getX() > Units.inchesToMeters(182.11) && driveTrain.getStatePose().getX() < Units.inchesToMeters(469.11)) { //If the robot is in the neutral Zone shuttle targetSpeeds.omegaRadiansPerSecond = driveTrain.getRotationalVelocityWhileShuttlingNotHub(shooter, upShuttleVector, downShuttleVector, controller); } else { //Else it must be in the alliance zone so toggle hub diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 569acf2..d1933bb 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -286,7 +286,7 @@ public ChassisSpeeds getHIDspeedsMPS(CommandPS5Controller driverController) { yAxis = MathUtil.applyDeadband(yAxis, Constants.Swerve.kDeadband); rotation = MathUtil.applyDeadband(rotation, Constants.Swerve.kDeadband); double maxSpeed = 0; - if(Math.abs(driverController.getRightY()) > 0.7) { + if(Math.abs(driverController.getRightY()) > 0.7 && getStatePose().getX() > Units.inchesToMeters(182.11) && getStatePose().getX() < Units.inchesToMeters(469.11)) { maxSpeed = 1; } else { maxSpeed = Constants.Swerve.maxSpeed;