From cc33a370ce720146315101689f4dd5527701757a Mon Sep 17 00:00:00 2001 From: PotmanNob Date: Sat, 3 Aug 2024 22:08:08 -0500 Subject: [PATCH 1/5] added autoAlign code --- src/main/java/frc/robot/Constants.java | 30 ++++ src/main/java/frc/robot/Robot.java | 2 + .../frc/robot/subsystems/drive/AutoAlign.java | 135 ++++++++++++++++++ .../drive/AutoAlignInstruction.java | 16 +++ .../frc/robot/subsystems/manager/Manager.java | 2 + 5 files changed, 185 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/drive/AutoAlign.java create mode 100644 src/main/java/frc/robot/subsystems/drive/AutoAlignInstruction.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4a130bc..9042870 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -14,7 +14,10 @@ package frc.robot; import com.pathplanner.lib.util.PIDConstants; + +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.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; @@ -320,4 +323,31 @@ public static final class NoteSim { public static final Translation3d GRAVITY_TRANSLATION3D = new Translation3d(0, 0, 9.8); public static final double OUT_OF_FIELD_MARGIN = .025; } + public static final class AutoAlign { + public static final PIDConstants TRANSLATIONAL_PID = new PIDConstants(3, 0, 0); + public static final PIDConstants ROTATIONAL_PID = new PIDConstants(5, 0, 0); + public static final double TIME_TO_FEED = 1.0; // seconds + public static final double TIME_FOR_SCORING = .5; // seconds + + public static final Pose2d redAmpSpeakerPose = + new Pose2d(15.59, 6.644, new Rotation2d(Math.toRadians(120.5))); + public static final Pose2d blueAmpSpeakerPose = + new Pose2d(0.909, 6.644, new Rotation2d(Math.toRadians(55.5))); + public static final Pose2d redSourceSpeakerPose = + new Pose2d(15.636, 4.39, new Rotation2d(Math.toRadians(-122.5))); + public static final Pose2d blueSourceSpeakerPose = + new Pose2d(0.864, 4.39, new Rotation2d(Math.toRadians(-62.5))); + public static final Pose2d redAmpPose = + new Pose2d(14.7, 7.72, new Rotation2d(Math.toRadians(-90))); + public static final Pose2d blueAmpPose = + new Pose2d(1.85, 7.72, new Rotation2d(Math.toRadians(-90))); + + public static final Pose2d blueSpeakerPose = + new Pose2d(1.45, 5.50, new Rotation2d(Math.toRadians(0))); + public static final Pose2d redSpeakerPose = + new Pose2d(15.1, 5.50, new Rotation2d(Math.toRadians(180))); + + public static final double TRANSLATION_ERROR_MARGIN = 0.4; + public static final double ROTATION_ERROR_MARGIN = 0.4; + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 34958f8..19facff 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -20,6 +20,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.commands.AutoCommands; +import frc.robot.subsystems.drive.AutoAlign; import frc.robot.subsystems.manager.*; import frc.robot.util.NoteSimulator; import org.littletonrobotics.junction.LogFileUtil; @@ -51,6 +52,7 @@ public class Robot extends LoggedRobot { @Override public void robotInit() { managerSubsystem = new Manager(); + AutoAlign.setManager(managerSubsystem);; NamedCommands.registerCommand("Intaking", autoCommands.intaking()); NamedCommands.registerCommand("Shooting", autoCommands.shooting()); diff --git a/src/main/java/frc/robot/subsystems/drive/AutoAlign.java b/src/main/java/frc/robot/subsystems/drive/AutoAlign.java new file mode 100644 index 0000000..a0b1562 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/AutoAlign.java @@ -0,0 +1,135 @@ +package frc.robot.subsystems.drive; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.Constants; +import frc.robot.subsystems.manager.Manager; +import frc.robot.subsystems.manager.ManagerStates; + +public class AutoAlign { + private static PIDController xPIDController = + new PIDController( + Constants.AutoAlign.TRANSLATIONAL_PID.kP, + Constants.AutoAlign.TRANSLATIONAL_PID.kI, + Constants.AutoAlign.TRANSLATIONAL_PID.kD); + private static PIDController yPIDController = + new PIDController( + Constants.AutoAlign.TRANSLATIONAL_PID.kP, + Constants.AutoAlign.TRANSLATIONAL_PID.kI, + Constants.AutoAlign.TRANSLATIONAL_PID.kD); + private static final PIDController rotationalPIDController = + new PIDController( + Constants.AutoAlign.ROTATIONAL_PID.kP, + Constants.AutoAlign.ROTATIONAL_PID.kI, + Constants.AutoAlign.ROTATIONAL_PID.kD); + private static Pose2d targetPose2d; + private static AutoAlignInstruction autoAlignInstruction; + private static Timer timer = new Timer(); + private static boolean setStateAlready; + + private static Drive drive; + private static Manager manager; + + public static void setTargetPose(Pose2d target) { + targetPose2d = target; + } + + public static void setDrive(Drive inputDrive) { + drive = inputDrive; + } + + public static void setManager(Manager inputManager) { + manager = inputManager; + } + + // calculate chassissSpeed obj + public static void calculateChassisSpeed() { + Pose2d currentPose2d = drive.getPose(); + + if (!nearSetPoint()) { + manager.setState(autoAlignInstruction.duringDrive); + + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + xPIDController.calculate(currentPose2d.getX(), targetPose2d.getX()), + yPIDController.calculate(currentPose2d.getY(), targetPose2d.getY()), + rotationalPIDController.calculate( + currentPose2d.getRotation().getRadians(), + targetPose2d.getRotation().getRadians()), + drive.getRotation())); + } else { + // this is the only way i could think of to make the code pretty. please don't murder me + // Amp bar needs time to be able to be fed + if (timer.hasElapsed(Constants.AutoAlign.TIME_FOR_SCORING) && setStateAlready) { + manager.setState(ManagerStates.IDLE); + drive.setState(DriveStates.REGULAR_DRIVE); + + } else if (autoAlignInstruction == AutoAlignInstruction.SCORE_AMP_BAR + && timer.hasElapsed(Constants.AutoAlign.TIME_TO_FEED)) { + manager.setState(autoAlignInstruction.atSetPoint); + + } else if (autoAlignInstruction == AutoAlignInstruction.SHOOT + && timer.hasElapsed(Constants.AutoAlign.TIME_TO_FEED)) { + manager.setState(autoAlignInstruction.atSetPoint); + } + + if (!setStateAlready) { + setStateAlready = true; + timer.reset(); + timer.start(); + } + } + } + + public static boolean nearSetPoint() { + Pose2d currentPose2d = drive.getPose(); + + if (Math.abs(currentPose2d.getX() - targetPose2d.getX()) + < Constants.AutoAlign.TRANSLATION_ERROR_MARGIN + && Math.abs(currentPose2d.getY() - targetPose2d.getY()) + < Constants.AutoAlign.TRANSLATION_ERROR_MARGIN + && Math.abs( + currentPose2d.getRotation().getDegrees() - targetPose2d.getRotation().getDegrees()) + < Constants.AutoAlign.ROTATION_ERROR_MARGIN) { + return true; + } + return false; + } + + public static void periodic() { + if (Constants.operatorController.getLeftBumper()) { + setTargetPose( + (DriverStation.getAlliance().get() == DriverStation.Alliance.Red) + ? Constants.AutoAlign.redSourceSpeakerPose + : Constants.AutoAlign.blueAmpSpeakerPose); + autoAlignInstruction = AutoAlignInstruction.SHOOT; + setStateAlready = false; + drive.setState(DriveStates.AUTO_ALIGN); + } else if (Constants.operatorController.getRightBumper()) { + setTargetPose( + (DriverStation.getAlliance().get() == DriverStation.Alliance.Red) + ? Constants.AutoAlign.redAmpSpeakerPose + : Constants.AutoAlign.blueSourceSpeakerPose); + autoAlignInstruction = AutoAlignInstruction.SHOOT; + setStateAlready = false; + drive.setState(DriveStates.AUTO_ALIGN); + } else if (Constants.operatorController.getAButton()) { + setTargetPose( + (DriverStation.getAlliance().get() == DriverStation.Alliance.Red) + ? Constants.AutoAlign.redAmpPose + : Constants.AutoAlign.blueAmpPose); + autoAlignInstruction = AutoAlignInstruction.SCORE_AMP_BAR; + timer.restart(); + timer.start(); + setStateAlready = false; + drive.setState(DriveStates.AUTO_ALIGN); + } else if (Constants.operatorController.getXButton()) { + drive.setState(DriveStates.REGULAR_DRIVE); + } + } + // #TODO add drive to amp bar and auto shoot. + // create timer for feeding and holding +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/AutoAlignInstruction.java b/src/main/java/frc/robot/subsystems/drive/AutoAlignInstruction.java new file mode 100644 index 0000000..49caf64 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/AutoAlignInstruction.java @@ -0,0 +1,16 @@ +package frc.robot.subsystems.drive; + +import frc.robot.subsystems.manager.ManagerStates; + +public enum AutoAlignInstruction { + SCORE_AMP_BAR(ManagerStates.FEED_AMP, ManagerStates.SCORE_AMP), + SHOOT(ManagerStates.SPINNING_UP, ManagerStates.SHOOTING); + + AutoAlignInstruction(ManagerStates duringDrive, ManagerStates atSetPoint) { + this.duringDrive = duringDrive; + this.atSetPoint = atSetPoint; + } + + ManagerStates duringDrive; + ManagerStates atSetPoint; +} diff --git a/src/main/java/frc/robot/subsystems/manager/Manager.java b/src/main/java/frc/robot/subsystems/manager/Manager.java index abb210f..4f3b6f5 100644 --- a/src/main/java/frc/robot/subsystems/manager/Manager.java +++ b/src/main/java/frc/robot/subsystems/manager/Manager.java @@ -3,6 +3,7 @@ import frc.robot.Constants; import frc.robot.subsystems.*; import frc.robot.subsystems.ampBar.*; +import frc.robot.subsystems.drive.AutoAlign; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.GyroIO; import frc.robot.subsystems.drive.GyroIOPigeon2; @@ -66,6 +67,7 @@ public Manager() { } NoteSimulator.setDrive(driveSubsystem); + AutoAlign.setDrive(driveSubsystem); // State Transitions (Nothing Automatic YET) From a621c45eec17e3739bf62ef5c03d78024d9f4b73 Mon Sep 17 00:00:00 2001 From: PotmanNob Date: Sat, 3 Aug 2024 22:14:18 -0500 Subject: [PATCH 2/5] syntax fix --- src/main/java/frc/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 19facff..79789a0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -52,7 +52,7 @@ public class Robot extends LoggedRobot { @Override public void robotInit() { managerSubsystem = new Manager(); - AutoAlign.setManager(managerSubsystem);; + AutoAlign.setManager(managerSubsystem); NamedCommands.registerCommand("Intaking", autoCommands.intaking()); NamedCommands.registerCommand("Shooting", autoCommands.shooting()); From 89acd2d8a64707daa11b2d7a56572a87f061e7d9 Mon Sep 17 00:00:00 2001 From: github-actions Date: Sun, 4 Aug 2024 14:58:34 +0000 Subject: [PATCH 3/5] Apply Prettier format --- src/main/java/frc/robot/Constants.java | 81 +++--- .../frc/robot/subsystems/drive/AutoAlign.java | 234 +++++++++--------- .../drive/AutoAlignInstruction.java | 16 +- 3 files changed, 184 insertions(+), 147 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9042870..d0fe8ef 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -14,7 +14,6 @@ package frc.robot; import com.pathplanner.lib.util.PIDConstants; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; @@ -323,31 +322,57 @@ public static final class NoteSim { public static final Translation3d GRAVITY_TRANSLATION3D = new Translation3d(0, 0, 9.8); public static final double OUT_OF_FIELD_MARGIN = .025; } - public static final class AutoAlign { - public static final PIDConstants TRANSLATIONAL_PID = new PIDConstants(3, 0, 0); - public static final PIDConstants ROTATIONAL_PID = new PIDConstants(5, 0, 0); - public static final double TIME_TO_FEED = 1.0; // seconds - public static final double TIME_FOR_SCORING = .5; // seconds - - public static final Pose2d redAmpSpeakerPose = - new Pose2d(15.59, 6.644, new Rotation2d(Math.toRadians(120.5))); - public static final Pose2d blueAmpSpeakerPose = - new Pose2d(0.909, 6.644, new Rotation2d(Math.toRadians(55.5))); - public static final Pose2d redSourceSpeakerPose = - new Pose2d(15.636, 4.39, new Rotation2d(Math.toRadians(-122.5))); - public static final Pose2d blueSourceSpeakerPose = - new Pose2d(0.864, 4.39, new Rotation2d(Math.toRadians(-62.5))); - public static final Pose2d redAmpPose = - new Pose2d(14.7, 7.72, new Rotation2d(Math.toRadians(-90))); - public static final Pose2d blueAmpPose = - new Pose2d(1.85, 7.72, new Rotation2d(Math.toRadians(-90))); - - public static final Pose2d blueSpeakerPose = - new Pose2d(1.45, 5.50, new Rotation2d(Math.toRadians(0))); - public static final Pose2d redSpeakerPose = - new Pose2d(15.1, 5.50, new Rotation2d(Math.toRadians(180))); - - public static final double TRANSLATION_ERROR_MARGIN = 0.4; - public static final double ROTATION_ERROR_MARGIN = 0.4; - } + + public static final class AutoAlign { + + public static final PIDConstants TRANSLATIONAL_PID = new PIDConstants(3, 0, 0); + public static final PIDConstants ROTATIONAL_PID = new PIDConstants(5, 0, 0); + public static final double TIME_TO_FEED = 1.0; // seconds + public static final double TIME_FOR_SCORING = .5; // seconds + + public static final Pose2d redAmpSpeakerPose = new Pose2d( + 15.59, + 6.644, + new Rotation2d(Math.toRadians(120.5)) + ); + public static final Pose2d blueAmpSpeakerPose = new Pose2d( + 0.909, + 6.644, + new Rotation2d(Math.toRadians(55.5)) + ); + public static final Pose2d redSourceSpeakerPose = new Pose2d( + 15.636, + 4.39, + new Rotation2d(Math.toRadians(-122.5)) + ); + public static final Pose2d blueSourceSpeakerPose = new Pose2d( + 0.864, + 4.39, + new Rotation2d(Math.toRadians(-62.5)) + ); + public static final Pose2d redAmpPose = new Pose2d( + 14.7, + 7.72, + new Rotation2d(Math.toRadians(-90)) + ); + public static final Pose2d blueAmpPose = new Pose2d( + 1.85, + 7.72, + new Rotation2d(Math.toRadians(-90)) + ); + + public static final Pose2d blueSpeakerPose = new Pose2d( + 1.45, + 5.50, + new Rotation2d(Math.toRadians(0)) + ); + public static final Pose2d redSpeakerPose = new Pose2d( + 15.1, + 5.50, + new Rotation2d(Math.toRadians(180)) + ); + + public static final double TRANSLATION_ERROR_MARGIN = 0.4; + public static final double ROTATION_ERROR_MARGIN = 0.4; + } } diff --git a/src/main/java/frc/robot/subsystems/drive/AutoAlign.java b/src/main/java/frc/robot/subsystems/drive/AutoAlign.java index a0b1562..2484354 100644 --- a/src/main/java/frc/robot/subsystems/drive/AutoAlign.java +++ b/src/main/java/frc/robot/subsystems/drive/AutoAlign.java @@ -10,126 +10,138 @@ import frc.robot.subsystems.manager.ManagerStates; public class AutoAlign { - private static PIDController xPIDController = - new PIDController( - Constants.AutoAlign.TRANSLATIONAL_PID.kP, - Constants.AutoAlign.TRANSLATIONAL_PID.kI, - Constants.AutoAlign.TRANSLATIONAL_PID.kD); - private static PIDController yPIDController = - new PIDController( - Constants.AutoAlign.TRANSLATIONAL_PID.kP, - Constants.AutoAlign.TRANSLATIONAL_PID.kI, - Constants.AutoAlign.TRANSLATIONAL_PID.kD); - private static final PIDController rotationalPIDController = - new PIDController( - Constants.AutoAlign.ROTATIONAL_PID.kP, - Constants.AutoAlign.ROTATIONAL_PID.kI, - Constants.AutoAlign.ROTATIONAL_PID.kD); - private static Pose2d targetPose2d; - private static AutoAlignInstruction autoAlignInstruction; - private static Timer timer = new Timer(); - private static boolean setStateAlready; - private static Drive drive; - private static Manager manager; + private static PIDController xPIDController = new PIDController( + Constants.AutoAlign.TRANSLATIONAL_PID.kP, + Constants.AutoAlign.TRANSLATIONAL_PID.kI, + Constants.AutoAlign.TRANSLATIONAL_PID.kD + ); + private static PIDController yPIDController = new PIDController( + Constants.AutoAlign.TRANSLATIONAL_PID.kP, + Constants.AutoAlign.TRANSLATIONAL_PID.kI, + Constants.AutoAlign.TRANSLATIONAL_PID.kD + ); + private static final PIDController rotationalPIDController = new PIDController( + Constants.AutoAlign.ROTATIONAL_PID.kP, + Constants.AutoAlign.ROTATIONAL_PID.kI, + Constants.AutoAlign.ROTATIONAL_PID.kD + ); + private static Pose2d targetPose2d; + private static AutoAlignInstruction autoAlignInstruction; + private static Timer timer = new Timer(); + private static boolean setStateAlready; - public static void setTargetPose(Pose2d target) { - targetPose2d = target; - } + private static Drive drive; + private static Manager manager; - public static void setDrive(Drive inputDrive) { - drive = inputDrive; - } + public static void setTargetPose(Pose2d target) { + targetPose2d = target; + } - public static void setManager(Manager inputManager) { - manager = inputManager; - } + public static void setDrive(Drive inputDrive) { + drive = inputDrive; + } - // calculate chassissSpeed obj - public static void calculateChassisSpeed() { - Pose2d currentPose2d = drive.getPose(); + public static void setManager(Manager inputManager) { + manager = inputManager; + } - if (!nearSetPoint()) { - manager.setState(autoAlignInstruction.duringDrive); + // calculate chassissSpeed obj + public static void calculateChassisSpeed() { + Pose2d currentPose2d = drive.getPose(); - drive.runVelocity( - ChassisSpeeds.fromFieldRelativeSpeeds( - xPIDController.calculate(currentPose2d.getX(), targetPose2d.getX()), - yPIDController.calculate(currentPose2d.getY(), targetPose2d.getY()), - rotationalPIDController.calculate( - currentPose2d.getRotation().getRadians(), - targetPose2d.getRotation().getRadians()), - drive.getRotation())); - } else { - // this is the only way i could think of to make the code pretty. please don't murder me - // Amp bar needs time to be able to be fed - if (timer.hasElapsed(Constants.AutoAlign.TIME_FOR_SCORING) && setStateAlready) { - manager.setState(ManagerStates.IDLE); - drive.setState(DriveStates.REGULAR_DRIVE); + if (!nearSetPoint()) { + manager.setState(autoAlignInstruction.duringDrive); - } else if (autoAlignInstruction == AutoAlignInstruction.SCORE_AMP_BAR - && timer.hasElapsed(Constants.AutoAlign.TIME_TO_FEED)) { - manager.setState(autoAlignInstruction.atSetPoint); + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + xPIDController.calculate(currentPose2d.getX(), targetPose2d.getX()), + yPIDController.calculate(currentPose2d.getY(), targetPose2d.getY()), + rotationalPIDController.calculate( + currentPose2d.getRotation().getRadians(), + targetPose2d.getRotation().getRadians() + ), + drive.getRotation() + ) + ); + } else { + // this is the only way i could think of to make the code pretty. please don't murder me + // Amp bar needs time to be able to be fed + if (timer.hasElapsed(Constants.AutoAlign.TIME_FOR_SCORING) && setStateAlready) { + manager.setState(ManagerStates.IDLE); + drive.setState(DriveStates.REGULAR_DRIVE); + } else if ( + autoAlignInstruction == AutoAlignInstruction.SCORE_AMP_BAR && + timer.hasElapsed(Constants.AutoAlign.TIME_TO_FEED) + ) { + manager.setState(autoAlignInstruction.atSetPoint); + } else if ( + autoAlignInstruction == AutoAlignInstruction.SHOOT && + timer.hasElapsed(Constants.AutoAlign.TIME_TO_FEED) + ) { + manager.setState(autoAlignInstruction.atSetPoint); + } - } else if (autoAlignInstruction == AutoAlignInstruction.SHOOT - && timer.hasElapsed(Constants.AutoAlign.TIME_TO_FEED)) { - manager.setState(autoAlignInstruction.atSetPoint); - } + if (!setStateAlready) { + setStateAlready = true; + timer.reset(); + timer.start(); + } + } + } - if (!setStateAlready) { - setStateAlready = true; - timer.reset(); - timer.start(); - } - } - } + public static boolean nearSetPoint() { + Pose2d currentPose2d = drive.getPose(); - public static boolean nearSetPoint() { - Pose2d currentPose2d = drive.getPose(); + if ( + Math.abs(currentPose2d.getX() - targetPose2d.getX()) < + Constants.AutoAlign.TRANSLATION_ERROR_MARGIN && + Math.abs(currentPose2d.getY() - targetPose2d.getY()) < + Constants.AutoAlign.TRANSLATION_ERROR_MARGIN && + Math.abs( + currentPose2d.getRotation().getDegrees() - targetPose2d.getRotation().getDegrees() + ) < + Constants.AutoAlign.ROTATION_ERROR_MARGIN + ) { + return true; + } + return false; + } - if (Math.abs(currentPose2d.getX() - targetPose2d.getX()) - < Constants.AutoAlign.TRANSLATION_ERROR_MARGIN - && Math.abs(currentPose2d.getY() - targetPose2d.getY()) - < Constants.AutoAlign.TRANSLATION_ERROR_MARGIN - && Math.abs( - currentPose2d.getRotation().getDegrees() - targetPose2d.getRotation().getDegrees()) - < Constants.AutoAlign.ROTATION_ERROR_MARGIN) { - return true; - } - return false; - } - - public static void periodic() { - if (Constants.operatorController.getLeftBumper()) { - setTargetPose( - (DriverStation.getAlliance().get() == DriverStation.Alliance.Red) - ? Constants.AutoAlign.redSourceSpeakerPose - : Constants.AutoAlign.blueAmpSpeakerPose); - autoAlignInstruction = AutoAlignInstruction.SHOOT; - setStateAlready = false; - drive.setState(DriveStates.AUTO_ALIGN); - } else if (Constants.operatorController.getRightBumper()) { - setTargetPose( - (DriverStation.getAlliance().get() == DriverStation.Alliance.Red) - ? Constants.AutoAlign.redAmpSpeakerPose - : Constants.AutoAlign.blueSourceSpeakerPose); - autoAlignInstruction = AutoAlignInstruction.SHOOT; - setStateAlready = false; - drive.setState(DriveStates.AUTO_ALIGN); - } else if (Constants.operatorController.getAButton()) { - setTargetPose( - (DriverStation.getAlliance().get() == DriverStation.Alliance.Red) - ? Constants.AutoAlign.redAmpPose - : Constants.AutoAlign.blueAmpPose); - autoAlignInstruction = AutoAlignInstruction.SCORE_AMP_BAR; - timer.restart(); - timer.start(); - setStateAlready = false; - drive.setState(DriveStates.AUTO_ALIGN); - } else if (Constants.operatorController.getXButton()) { - drive.setState(DriveStates.REGULAR_DRIVE); - } - } - // #TODO add drive to amp bar and auto shoot. - // create timer for feeding and holding -} \ No newline at end of file + public static void periodic() { + if (Constants.operatorController.getLeftBumper()) { + setTargetPose( + (DriverStation.getAlliance().get() == DriverStation.Alliance.Red) + ? Constants.AutoAlign.redSourceSpeakerPose + : Constants.AutoAlign.blueAmpSpeakerPose + ); + autoAlignInstruction = AutoAlignInstruction.SHOOT; + setStateAlready = false; + drive.setState(DriveStates.AUTO_ALIGN); + } else if (Constants.operatorController.getRightBumper()) { + setTargetPose( + (DriverStation.getAlliance().get() == DriverStation.Alliance.Red) + ? Constants.AutoAlign.redAmpSpeakerPose + : Constants.AutoAlign.blueSourceSpeakerPose + ); + autoAlignInstruction = AutoAlignInstruction.SHOOT; + setStateAlready = false; + drive.setState(DriveStates.AUTO_ALIGN); + } else if (Constants.operatorController.getAButton()) { + setTargetPose( + (DriverStation.getAlliance().get() == DriverStation.Alliance.Red) + ? Constants.AutoAlign.redAmpPose + : Constants.AutoAlign.blueAmpPose + ); + autoAlignInstruction = AutoAlignInstruction.SCORE_AMP_BAR; + timer.restart(); + timer.start(); + setStateAlready = false; + drive.setState(DriveStates.AUTO_ALIGN); + } else if (Constants.operatorController.getXButton()) { + drive.setState(DriveStates.REGULAR_DRIVE); + } + } + // #TODO add drive to amp bar and auto shoot. + // create timer for feeding and holding +} diff --git a/src/main/java/frc/robot/subsystems/drive/AutoAlignInstruction.java b/src/main/java/frc/robot/subsystems/drive/AutoAlignInstruction.java index 49caf64..05ecdae 100644 --- a/src/main/java/frc/robot/subsystems/drive/AutoAlignInstruction.java +++ b/src/main/java/frc/robot/subsystems/drive/AutoAlignInstruction.java @@ -3,14 +3,14 @@ import frc.robot.subsystems.manager.ManagerStates; public enum AutoAlignInstruction { - SCORE_AMP_BAR(ManagerStates.FEED_AMP, ManagerStates.SCORE_AMP), - SHOOT(ManagerStates.SPINNING_UP, ManagerStates.SHOOTING); + SCORE_AMP_BAR(ManagerStates.FEED_AMP, ManagerStates.SCORE_AMP), + SHOOT(ManagerStates.SPINNING_UP, ManagerStates.SHOOTING); - AutoAlignInstruction(ManagerStates duringDrive, ManagerStates atSetPoint) { - this.duringDrive = duringDrive; - this.atSetPoint = atSetPoint; - } + AutoAlignInstruction(ManagerStates duringDrive, ManagerStates atSetPoint) { + this.duringDrive = duringDrive; + this.atSetPoint = atSetPoint; + } - ManagerStates duringDrive; - ManagerStates atSetPoint; + ManagerStates duringDrive; + ManagerStates atSetPoint; } From 8bdcf0b6cab3c219ce18aec429590770f47a363b Mon Sep 17 00:00:00 2001 From: PotmanNob Date: Tue, 6 Aug 2024 19:52:01 -0500 Subject: [PATCH 4/5] removed timer from autoAlign --- .../frc/robot/subsystems/drive/AutoAlign.java | 49 +++---------------- 1 file changed, 7 insertions(+), 42 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/AutoAlign.java b/src/main/java/frc/robot/subsystems/drive/AutoAlign.java index 2484354..33ab7ff 100644 --- a/src/main/java/frc/robot/subsystems/drive/AutoAlign.java +++ b/src/main/java/frc/robot/subsystems/drive/AutoAlign.java @@ -4,10 +4,8 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; import frc.robot.Constants; import frc.robot.subsystems.manager.Manager; -import frc.robot.subsystems.manager.ManagerStates; public class AutoAlign { @@ -28,8 +26,6 @@ public class AutoAlign { ); private static Pose2d targetPose2d; private static AutoAlignInstruction autoAlignInstruction; - private static Timer timer = new Timer(); - private static boolean setStateAlready; private static Drive drive; private static Manager manager; @@ -65,47 +61,23 @@ public static void calculateChassisSpeed() { ) ); } else { - // this is the only way i could think of to make the code pretty. please don't murder me - // Amp bar needs time to be able to be fed - if (timer.hasElapsed(Constants.AutoAlign.TIME_FOR_SCORING) && setStateAlready) { - manager.setState(ManagerStates.IDLE); - drive.setState(DriveStates.REGULAR_DRIVE); - } else if ( - autoAlignInstruction == AutoAlignInstruction.SCORE_AMP_BAR && - timer.hasElapsed(Constants.AutoAlign.TIME_TO_FEED) - ) { - manager.setState(autoAlignInstruction.atSetPoint); - } else if ( - autoAlignInstruction == AutoAlignInstruction.SHOOT && - timer.hasElapsed(Constants.AutoAlign.TIME_TO_FEED) - ) { manager.setState(autoAlignInstruction.atSetPoint); - } - - if (!setStateAlready) { - setStateAlready = true; - timer.reset(); - timer.start(); - } + drive.setState(DriveStates.REGULAR_DRIVE); } } public static boolean nearSetPoint() { Pose2d currentPose2d = drive.getPose(); - if ( + return ( Math.abs(currentPose2d.getX() - targetPose2d.getX()) < Constants.AutoAlign.TRANSLATION_ERROR_MARGIN && Math.abs(currentPose2d.getY() - targetPose2d.getY()) < - Constants.AutoAlign.TRANSLATION_ERROR_MARGIN && + Constants.AutoAlign.TRANSLATION_ERROR_MARGIN && Math.abs( - currentPose2d.getRotation().getDegrees() - targetPose2d.getRotation().getDegrees() - ) < - Constants.AutoAlign.ROTATION_ERROR_MARGIN - ) { - return true; - } - return false; + currentPose2d.getRotation().getDegrees() - targetPose2d.getRotation().getDegrees()) < + Constants.AutoAlign.ROTATION_ERROR_MARGIN + ); } public static void periodic() { @@ -116,7 +88,6 @@ public static void periodic() { : Constants.AutoAlign.blueAmpSpeakerPose ); autoAlignInstruction = AutoAlignInstruction.SHOOT; - setStateAlready = false; drive.setState(DriveStates.AUTO_ALIGN); } else if (Constants.operatorController.getRightBumper()) { setTargetPose( @@ -124,8 +95,7 @@ public static void periodic() { ? Constants.AutoAlign.redAmpSpeakerPose : Constants.AutoAlign.blueSourceSpeakerPose ); - autoAlignInstruction = AutoAlignInstruction.SHOOT; - setStateAlready = false; + autoAlignInstruction = AutoAlignInstruction.SHOOT;; drive.setState(DriveStates.AUTO_ALIGN); } else if (Constants.operatorController.getAButton()) { setTargetPose( @@ -134,14 +104,9 @@ public static void periodic() { : Constants.AutoAlign.blueAmpPose ); autoAlignInstruction = AutoAlignInstruction.SCORE_AMP_BAR; - timer.restart(); - timer.start(); - setStateAlready = false; drive.setState(DriveStates.AUTO_ALIGN); } else if (Constants.operatorController.getXButton()) { drive.setState(DriveStates.REGULAR_DRIVE); } } - // #TODO add drive to amp bar and auto shoot. - // create timer for feeding and holding } From a400908b15fd2cc6ea3ac72f1f219337c9536e19 Mon Sep 17 00:00:00 2001 From: github-actions Date: Wed, 7 Aug 2024 00:52:34 +0000 Subject: [PATCH 5/5] Apply Prettier format --- .../java/frc/robot/subsystems/drive/AutoAlign.java | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/AutoAlign.java b/src/main/java/frc/robot/subsystems/drive/AutoAlign.java index 33ab7ff..05fb574 100644 --- a/src/main/java/frc/robot/subsystems/drive/AutoAlign.java +++ b/src/main/java/frc/robot/subsystems/drive/AutoAlign.java @@ -61,8 +61,8 @@ public static void calculateChassisSpeed() { ) ); } else { - manager.setState(autoAlignInstruction.atSetPoint); - drive.setState(DriveStates.REGULAR_DRIVE); + manager.setState(autoAlignInstruction.atSetPoint); + drive.setState(DriveStates.REGULAR_DRIVE); } } @@ -73,10 +73,11 @@ public static boolean nearSetPoint() { Math.abs(currentPose2d.getX() - targetPose2d.getX()) < Constants.AutoAlign.TRANSLATION_ERROR_MARGIN && Math.abs(currentPose2d.getY() - targetPose2d.getY()) < - Constants.AutoAlign.TRANSLATION_ERROR_MARGIN && + Constants.AutoAlign.TRANSLATION_ERROR_MARGIN && Math.abs( - currentPose2d.getRotation().getDegrees() - targetPose2d.getRotation().getDegrees()) < - Constants.AutoAlign.ROTATION_ERROR_MARGIN + currentPose2d.getRotation().getDegrees() - targetPose2d.getRotation().getDegrees() + ) < + Constants.AutoAlign.ROTATION_ERROR_MARGIN ); } @@ -95,7 +96,7 @@ public static void periodic() { ? Constants.AutoAlign.redAmpSpeakerPose : Constants.AutoAlign.blueSourceSpeakerPose ); - autoAlignInstruction = AutoAlignInstruction.SHOOT;; + autoAlignInstruction = AutoAlignInstruction.SHOOT; drive.setState(DriveStates.AUTO_ALIGN); } else if (Constants.operatorController.getAButton()) { setTargetPose(