diff --git a/src/main/deploy/pathplanner/autos/Quick Shoot test.auto b/src/main/deploy/pathplanner/autos/Robot Reveal Night.auto similarity index 92% rename from src/main/deploy/pathplanner/autos/Quick Shoot test.auto rename to src/main/deploy/pathplanner/autos/Robot Reveal Night.auto index 1828ee3..848e046 100644 --- a/src/main/deploy/pathplanner/autos/Quick Shoot test.auto +++ b/src/main/deploy/pathplanner/autos/Robot Reveal Night.auto @@ -7,7 +7,7 @@ { "type": "named", "data": { - "name": "Robot Intake Down" + "name": "Robot Rev Up" } }, { diff --git a/src/main/deploy/pathplanner/autos/Shoot and Lidar test.auto b/src/main/deploy/pathplanner/autos/Shoot and Lidar test.auto deleted file mode 100644 index 21b7fe6..0000000 --- a/src/main/deploy/pathplanner/autos/Shoot and Lidar test.auto +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Quick Shoot" - } - }, - { - "type": "named", - "data": { - "name": "Robot Shoot" - } - }, - { - "type": "path", - "data": { - "pathName": "DriveDepot" - } - }, - { - "type": "named", - "data": { - "name": "Lidar Intake" - } - }, - { - "type": "path", - "data": { - "pathName": "Depot Shoot" - } - }, - { - "type": "named", - "data": { - "name": "Robot Shoot" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Quick Shoot.path b/src/main/deploy/pathplanner/paths/Quick Shoot.path index 9bbf6cd..36bd07b 100644 --- a/src/main/deploy/pathplanner/paths/Quick Shoot.path +++ b/src/main/deploy/pathplanner/paths/Quick Shoot.path @@ -3,29 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.636650602409638, - "y": 5.051277108433735 + "x": 3.6, + "y": 4.0 }, "prevControl": null, "nextControl": { - "x": 2.871710843373494, - "y": 5.215192771084337 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.3958487874465044, - "y": 4.7919115549215405 - }, - "prevControl": { - "x": 2.4481210651242846, - "y": 5.284764458740615 - }, - "nextControl": { - "x": 2.30527817403709, - "y": 3.937960057061342 + "x": 3.35, + "y": 4.0 }, "isLocked": false, "linkedName": null @@ -36,8 +20,8 @@ "y": 4.0 }, "prevControl": { - "x": 2.4025543758309, - "y": 4.230226736784185 + "x": 2.75, + "y": 4.0 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 20a0525..9aa6d00 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -30,6 +30,7 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc4388.robot.commands.MoveForTimeCommand; +import frc4388.robot.commands.alignment.AutoAlign; import frc4388.robot.constants.Constants; import frc4388.robot.constants.Constants.OIConstants; import frc4388.robot.constants.Constants.SimConstants.Mode; @@ -98,7 +99,7 @@ public class RobotContainer { private Command RobotIntakeDown = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Extended)) + new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Extended), m_robotIntake) ); private Command LidarIntake = new SequentialCommandGroup( @@ -117,34 +118,24 @@ public class RobotContainer { .andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)) ); - private Command RobotShoot = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted)), - new InstantCommand(() -> m_robotShooter.setShooterReady()), - new InstantCommand(()->m_robotIntake.setMode(IntakeMode.Idle)), - new WaitCommand(3), - new InstantCommand(()->m_robotShooter.setShooterShoot()), - new WaitCommand(5), - new InstantCommand(()->m_robotShooter.setShooterNOTShoot()), - new InstantCommand(() -> m_robotShooter.setShooterNotReady()) + private Command RobotReadyToShoot = new SequentialCommandGroup( + new InstantCommand(()-> m_robotIntake.setMode(IntakeMode.Idle), m_robotIntake), + new InstantCommand(() -> m_robotShooter.setShooterReady(), m_robotShooter) ); - // private Command RobotShoot = new SequentialCommandGroup( - // new InstantCommand(() -> System.out.println(m_robotLED.getMode())), - // new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.PARTY_TWINKLES), m_robotLED), - // new InstantCommand(() -> System.out.println(m_robotLED.getMode())), - // new WaitCommand(5), - // new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_RED), m_robotLED), - // new InstantCommand(() -> System.out.println(m_robotLED.getMode())) - // ); - - // private Command RobotShoot = new SequentialCommandGroup( - // new InstantCommand(() -> m_robotShooter.setMode(Shooter.ShooterMode.Active), m_robotShooter) - // ); - - // private Command RobotIntake = new SequentialCommandGroup( - // new InstantCommand(() -> m_robotIntake.setMode(Intake.IntakeMode.Down), m_robotIntake) - // ); + private Command RobotShoot = new SequentialCommandGroup( + new InstantCommand(() -> + new AutoAlign(m_robotSwerveDrive, m_vision, true)), + new InstantCommand(()-> m_robotShooter.setShooterShoot(), m_robotShooter), + new InstantCommand(()-> m_robotShooter.setShooterReady(), m_robotShooter), + new WaitCommand(3), + new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake), + new WaitCommand(2), + new InstantCommand(()->m_robotShooter.setShooterNOTShoot(), m_robotShooter), + new InstantCommand(() -> m_robotShooter.setShooterNotReady(), m_robotShooter) + ); + public RobotContainer() { configureButtonBindings(); @@ -163,6 +154,7 @@ public class RobotContainer { m_robotShooter.io.updateGains(); }, true); + NamedCommands.registerCommand("Robot Rev Up", RobotReadyToShoot); NamedCommands.registerCommand("Robot Shoot", RobotShoot); NamedCommands.registerCommand("Lidar Intake", LidarIntake); NamedCommands.registerCommand("Robot Intake Down", RobotIntakeDown); diff --git a/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java new file mode 100644 index 0000000..9ed2859 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java @@ -0,0 +1,143 @@ +package frc4388.robot.commands.alignment; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.constants.Constants.AutoConstants; +import frc4388.robot.subsystems.swerve.SwerveDrive; +import frc4388.robot.subsystems.vision.Vision; +import frc4388.utility.compute.FieldPositions; +import frc4388.utility.compute.TimesNegativeOne; +import frc4388.utility.structs.Gains; + +public class AutoAlign extends Command { + + private PID xPID = new PID(AutoConstants.XY_GAINS, 0); + private PID yPID = new PID(AutoConstants.XY_GAINS, 0); + private Pose2d targetpos; + + SwerveDrive swerveDrive; + Vision vision; + boolean waitVelocityZero; + + public AutoAlign(SwerveDrive swerveDrive, Vision vision, boolean waitVelocityZero) { + this.swerveDrive = swerveDrive; + this.vision = vision; + this.waitVelocityZero = waitVelocityZero && false; + addRequirements(swerveDrive); + } + + public static double tagRelativeXError = -1; + private static void setTagRelativeXError(double val){ + tagRelativeXError = val; + } + + @Override + public void initialize() { + xPID.initialize(); + yPID.initialize(); + this.targetpos = new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)); + } + + double xerr; + double yerr; + double roterr; + + double xoutput; + double youtput; + double rotoutput; + + @Override + public void execute() { + xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis); + yerr = TimesNegativeOne.invert(targetpos.getY() - vision.getPose2d().getY(), !TimesNegativeOne.YAxis); + + roterr = ((targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees())); + + if(roterr > 180){ + roterr -= 360; + }else if(roterr < -180){ + roterr += 360; + } + + + + SmartDashboard.putNumber("PID X Error", xerr); + SmartDashboard.putNumber("PID Y Error", yerr); + SmartDashboard.putNumber("PID Rot Error", roterr); + + xoutput = xPID.update(xerr); + youtput = yPID.update(yerr); + + xoutput *= Math.abs(xerr) < AutoConstants.XY_TOLERANCE ? 0 : 1; + youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1; + + + + Translation2d leftStick = new Translation2d( + Math.max(Math.min(-youtput, 1), -1), + Math.max(Math.min(-xoutput, 1), -1) + ); + + + + setTagRelativeXError( + new Translation2d(xerr, yerr).getAngle() + .rotateBy(targetpos.getRotation()) + .getCos()); + + swerveDrive.driveRelativeAngle(leftStick, targetpos.getRotation()); + } + + @Override + public final boolean isFinished() { + boolean finished = ( + (Math.abs(xerr) < AutoConstants.XY_TOLERANCE || Math.abs(xoutput) <= AutoConstants.MIN_XY_PID_OUTPUT) && + (Math.abs(yerr) < AutoConstants.XY_TOLERANCE || Math.abs(youtput) <= AutoConstants.MIN_XY_PID_OUTPUT) && + (Math.abs(roterr) < AutoConstants.ROT_TOLERANCE) && + (!waitVelocityZero || swerveDrive.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD) + ); + + if(finished) + swerveDrive.softStop(); + + return finished; + } + + + + + private class PID { + protected Gains gains; + private double output = 0; + + + /** Creates a new PelvicInflammatoryDisease. */ + public PID(Gains gains, double tolerance) { + this.gains = gains; + } + + // Called when the command is initially scheduled. + public final void initialize() { + output = 0; + } + + private double prevError, cumError = 0; + + // Called every time the scheduler runs while the command is scheduled. + public double update(double error) { + cumError += error * .02; // 20 ms + double delta = error - prevError; + + output = error * gains.kP; + output += cumError * gains.kI; + output += delta * gains.kD; + output += gains.kF; + + return output; + } + } + +} diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 30ca2ae..3b38cbf 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 86; - public static final String GIT_SHA = "b3f3ae444324c4d339e4aa5dc59fe75bd3d56558"; - public static final String GIT_DATE = "2026-02-23 23:28:25 MST"; + public static final int GIT_REVISION = 87; + public static final String GIT_SHA = "34a7ed050d2967603ebf12cd0bad0b7264aeae01"; + public static final String GIT_DATE = "2026-02-24 17:49:26 MST"; public static final String GIT_BRANCH = "AutoTesting"; - public static final String BUILD_DATE = "2026-02-24 17:44:43 MST"; - public static final long BUILD_UNIX_TIME = 1771980283653L; + public static final String BUILD_DATE = "2026-02-24 18:48:38 MST"; + public static final long BUILD_UNIX_TIME = 1771984118729L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 31bde6b..79fbf75 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -69,6 +69,7 @@ public class Shooter extends SubsystemBase { this.mode = ShooterMode.Ready; } + public void setShooterReadyFeeder() { this.mode = ShooterMode.ReadyFeeder; }