From 89acd2d8a64707daa11b2d7a56572a87f061e7d9 Mon Sep 17 00:00:00 2001 From: github-actions Date: Sun, 4 Aug 2024 14:58:34 +0000 Subject: [PATCH] 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; }