From c94aa839195fc6369bbb2b307ec25909cce19674 Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Mon, 4 Apr 2022 17:22:52 -0600 Subject: [PATCH] auto shtuff --- src/main/deploy/pathplanner/Diamond.path | 2 +- src/main/deploy/pathplanner/JMove.path | 1 + src/main/deploy/pathplanner/Move Forward.path | 2 +- src/main/java/frc4388/robot/Constants.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 38 +++++++++++++++---- .../commands/ShooterCommands/TrackTarget.java | 16 ++++---- .../frc4388/robot/subsystems/SwerveDrive.java | 15 +++++--- 7 files changed, 53 insertions(+), 23 deletions(-) create mode 100644 src/main/deploy/pathplanner/JMove.path diff --git a/src/main/deploy/pathplanner/Diamond.path b/src/main/deploy/pathplanner/Diamond.path index 3d5a9ca..a8e6760 100644 --- a/src/main/deploy/pathplanner/Diamond.path +++ b/src/main/deploy/pathplanner/Diamond.path @@ -1 +1 @@ -{"waypoints":[{"anchorPoint":{"x":3.0,"y":3.0},"prevControl":null,"nextControl":{"x":3.410442907302426,"y":3.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.5,"y":4.5},"prevControl":{"x":4.288229266113705,"y":4.529967680625016},"nextControl":{"x":4.67537248192198,"y":4.475183036710464},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":6.0,"y":3.0},"prevControl":{"x":5.973638727554094,"y":3.050086417647222},"nextControl":{"x":6.10510365848318,"y":2.8003030488819602},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.5,"y":1.5029823163094613},"prevControl":{"x":4.6862524414704,"y":1.722990549429376},"nextControl":{"x":4.3137475585296,"y":1.2829740831895466},"holonomicAngle":180.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.0,"y":3.0},"prevControl":{"x":3.5480060960121915,"y":2.4748364324314855},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":5.0,"maxAcceleration":5.0,"isReversed":null} \ No newline at end of file +{"waypoints":[{"anchorPoint":{"x":3.0,"y":3.0},"prevControl":null,"nextControl":{"x":3.414538474646569,"y":3.4492625947277213},"holonomicAngle":180.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.5,"y":4.5},"prevControl":{"x":4.514135610549702,"y":4.641457384180591},"nextControl":{"x":4.514135610549702,"y":4.641457384180591},"holonomicAngle":180.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":6.0,"y":3.0},"prevControl":{"x":5.973638727554094,"y":3.050086417647222},"nextControl":{"x":5.973638727554094,"y":3.050086417647222},"holonomicAngle":180.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.5,"y":1.5029823163094613},"prevControl":{"x":4.514135610549702,"y":1.2732177363088903},"nextControl":{"x":4.514135610549702,"y":1.2732177363088903},"holonomicAngle":180.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.0,"y":3.0},"prevControl":{"x":3.5480060960121915,"y":2.4748364324314855},"nextControl":null,"holonomicAngle":180.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/JMove.path b/src/main/deploy/pathplanner/JMove.path new file mode 100644 index 0000000..8143427 --- /dev/null +++ b/src/main/deploy/pathplanner/JMove.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":7.6740305379344935,"y":1.7593554174450114},"prevControl":null,"nextControl":{"x":7.6740305379344935,"y":1.0066434217868019},"holonomicAngle":90.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.662455831240775,"y":0.7},"prevControl":{"x":7.662455831240775,"y":1.4061519765883657},"nextControl":null,"holonomicAngle":90.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Move Forward.path b/src/main/deploy/pathplanner/Move Forward.path index 1e2c78c..724ef2f 100644 --- a/src/main/deploy/pathplanner/Move Forward.path +++ b/src/main/deploy/pathplanner/Move Forward.path @@ -1 +1 @@ -{"waypoints":[{"anchorPoint":{"x":3.0,"y":2.0},"prevControl":null,"nextControl":{"x":3.0148639019281944,"y":2.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":2.0},"prevControl":{"x":4.45976632803168,"y":2.0},"nextControl":null,"holonomicAngle":180.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null} \ No newline at end of file +{"waypoints":[{"anchorPoint":{"x":3.0,"y":2.0024242580130727},"prevControl":null,"nextControl":{"x":3.0148639019281944,"y":2.0024242580130727},"holonomicAngle":180.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":2.0},"prevControl":{"x":4.45976632803168,"y":2.0},"nextControl":null,"holonomicAngle":180.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e517fb3..2f917d4 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -85,7 +85,7 @@ public final class Constants { // swerve auto constants public static final PIDController X_CONTROLLER = new PIDController(0.5, 0.0, 0.0); public static final PIDController Y_CONTROLLER = new PIDController(2.0, 0.0, 0.0); - public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(2.0, 0, 0.01,//0.1, 0.3, + public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(15.0, 0, 0.0,//0.1, 0.3, new TrapezoidProfile.Constraints(Math.PI, Math.PI/2)); public static final boolean PATH_RECORD_VELOCITY = true; public static final double PATH_MAX_VELOCITY = 5.0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d4a4e3a..48f3289 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -34,6 +34,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -347,8 +349,10 @@ public class RobotContainer { // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) // .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false)); + // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + // .whenPressed(new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - 10, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0)); new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); + .whileHeld(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret)); // * aim with turret to target); // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true))); @@ -447,12 +451,13 @@ public class RobotContainer { * @param inputs strings (paths) or commands you want to run (in order) * @return array of commands */ - public Command[] buildAuto(Double maxVel, Double maxAccel, Object... inputs) { + public SequentialCommandGroup buildAuto(Double maxVel, Double maxAccel, Object... inputs) { maxVel = Objects.requireNonNullElse(maxVel, SwerveDriveConstants.PATH_MAX_VELOCITY); maxAccel = Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.PATH_MAX_ACCELERATION); ArrayList commands = new ArrayList(); - commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); + // commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); + // commands.add(new InstantCommand(() -> m_robotSwerveDrive.m, m_robotSwerveDrive)); PIDController xController = SwerveDriveConstants.X_CONTROLLER; PIDController yController = SwerveDriveConstants.Y_CONTROLLER; @@ -485,10 +490,11 @@ public class RobotContainer { } } - commands.add(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0, 0, 0, 0, true), m_robotSwerveDrive)); + commands.add(new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive)); Command[] ret = new Command[commands.size()]; ret = commands.toArray(ret); - return ret; + SequentialCommandGroup seqCG = new SequentialCommandGroup(ret); + return seqCG; } /** @@ -522,7 +528,13 @@ public class RobotContainer { SequentialCommandGroup weirdAutoShootingGroup = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), new ParallelCommandGroup( new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), - new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 5.0) + new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 2.0, true) + )); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead. + + SequentialCommandGroup weirdAutoShootingGroup2 = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), + new ParallelCommandGroup( + new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), + new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 2.0, true) )); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead. // ! DRIVE BACKWARDS AND SHOOT (HOPEFULLY) @@ -605,7 +617,19 @@ public class RobotContainer { // ); // ! PathPlanner Testing - return new SequentialCommandGroup(buildAuto(1.0, 1.0, "Move Forward")); + ParallelDeadlineGroup intakeWithPath = new ParallelDeadlineGroup(new RunCommandForTime(new RunCommand(() -> m_robotIntake.runAtOutput(-1.0), m_robotIntake), 3.0, true), + new RunCommand(() -> m_robotSerializer.setSerializer(0.8), m_robotSerializer), + buildAuto(1.0, 1.0, "JMove")); + // return new SequentialCommandGroup(buildAuto(1.0, 1.0, "Move Forward")); + return new SequentialCommandGroup(new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true), // * aim with turret to target + weirdAutoShootingGroup, // * shoot + new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.5, true), + new ExtenderIntakeGroup(m_robotIntake, m_robotExtender), + intakeWithPath, + weirdAutoShootingGroup2, + // new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true)); // * aim with turret to target); // * shoot + new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.5, true)); + // return new SequentialCommandGroup(buildAuto(1.0, 1.0, "Diamond")); } diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index f7a67b7..a9388d3 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -196,14 +196,14 @@ public class TrackTarget extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - // if (this.isAuto) { - // if (targetLocked && !timerStarted) { - // timerStarted = true; - // startTime = System.currentTimeMillis(); - // } - // return (targetLocked && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance)); - // } else { + if (this.isAuto) { + if (targetLocked && !timerStarted) { + timerStarted = true; + startTime = System.currentTimeMillis(); + } + return (targetLocked && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance)); + } else { return false; - // } + } } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index e0c1994..1ca921f 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -177,14 +177,14 @@ public class SwerveDrive extends SubsystemBase { public void periodic() { updateOdometry(); - // updateSmartDash(); + updateSmartDash(); // SmartDashboard.putNumber("Pigeon getRotation2d", m_gyro.getRotation2d().getDegrees()); // SmartDashboard.putNumber("Pigeon getAngle", m_gyro.getAngle()); // SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw()); // SmartDashboard.putNumber("Pigeon Yaw (0 to 360)", m_gyro.getYaw() % 360); - m_field.setRobotPose(m_odometry.getPoseMeters()); + m_field.setRobotPose(getOdometry()); super.periodic(); } @@ -220,6 +220,11 @@ public class SwerveDrive extends SubsystemBase { // return m_poseEstimator.getEstimatedPosition(); } + public Pose2d getAutoOdo() { + Pose2d workingPose = getOdometry(); + return new Pose2d(-workingPose.getX(), workingPose.getY(), workingPose.getRotation()); + } + /** * Gets the current gyro using regression formula. * @@ -244,13 +249,13 @@ public class SwerveDrive extends SubsystemBase { * Updates the field relative position of the robot. */ public void updateOdometry() { - Rotation2d actualDWI = new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI/2)); - Rotation2d actual = new Rotation2d(-1 * m_gyro.getRotation2d().getRadians()); + Rotation2d actualDWI = new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2)); //+ (Math.PI/2)); + Rotation2d actual = new Rotation2d(m_gyro.getRotation2d().getRadians()); SmartDashboard.putNumber("AUTO ACTUAL GYRO", actual.getDegrees()); SmartDashboard.putNumber("AUTO DWI GYRO", actual.getDegrees()); - m_odometry.update( m_gyro.getRotation2d(),//new Rotation2d((2 * Math.PI) - getRegGyro().getRadians()), + m_odometry.update( actual,//m_gyro.getRotation2d(),//new Rotation2d((2 * Math.PI) - getRegGyro().getRadians()), modules[0].getState(), modules[1].getState(), modules[2].getState(),