From 5e30c924be99eaeb53bdf6ed6bf748cf462aeea0 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Sat, 2 Apr 2022 13:17:12 -0600 Subject: [PATCH 1/4] limelight delay --- src/main/java/frc4388/robot/subsystems/VisionOdometry.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java index 85f4b82..0ff741f 100644 --- a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java +++ b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java @@ -44,7 +44,10 @@ public class VisionOdometry extends SubsystemBase { * @param shooter The turret subsystem */ public VisionOdometry(SwerveDrive drive, Turret shooter) { - m_camera = new PhotonCamera(VisionConstants.NAME); + do { + m_camera = new PhotonCamera(VisionConstants.NAME); + } while(m_camera.getLatestResult().getLatencyMillis() > 0.d); + m_drive = drive; m_shooter = shooter; From 092134ca2c9b2a02a3c8d7fb2af3d5b655e54639 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 2 Apr 2022 13:53:47 -0600 Subject: [PATCH 2/4] dpad try --- .../controller/DeadbandedXboxController.java | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java index 5b1bc97..1c2ce22 100644 --- a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java +++ b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java @@ -8,10 +8,32 @@ import frc4388.robot.Constants.OIConstants; public class DeadbandedXboxController extends XboxController { public DeadbandedXboxController(int port) { super(port); } + @Override public double getLeftX() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()).getX(); } @Override public double getLeftY() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()).getY(); } @Override public double getRightX() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()).getX(); } @Override public double getRightY() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()).getY(); } + + public int getDpadAngle() { + return getPOV(0); + } + + public boolean getDPadLeft(){ + return (getRawAxis(6) < -0.9); + } + + public boolean getDPadRight(){ + return (getRawAxis(6) > 0.9); + } + + public boolean getDPadTop(){ + return (getRawAxis(6) < 0.9); + } + + public boolean getDPadBottom(){ + return (getRawAxis(6) > -0.9); + } + public static Translation2d skewToDeadzonedCircle(double x, double y) { Translation2d translation2d = new Translation2d(x, y); double magnitude = translation2d.getNorm(); From 2d9d4fc426a0ad2b595ef3474c743d557574e2c9 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 2 Apr 2022 14:45:22 -0600 Subject: [PATCH 3/4] odometry is back baby --- .../java/frc4388/robot/RobotContainer.java | 10 ++++++++ .../frc4388/robot/subsystems/SwerveDrive.java | 23 ++++++++----------- 2 files changed, 19 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index f0c5b0c..f8eb76d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -458,6 +458,7 @@ public class RobotContainer { maxAccel = Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.PATH_MAX_ACCELERATION); ArrayList commands = new ArrayList(); + commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); PIDController xController = SwerveDriveConstants.X_CONTROLLER; PIDController yController = SwerveDriveConstants.Y_CONTROLLER; @@ -503,6 +504,15 @@ public class RobotContainer { */ public Command getAutonomousCommand() { + // ! PathPlanner Testing + if (!this.currentClimberMode.equals(null)) { + return new SequentialCommandGroup(buildAuto(1.0, 1.0, "Drive Forward")); + } + + if (!this.currentClimberMode.equals(null)) { + return new SequentialCommandGroup(buildAuto(1.0, 1.0, "Diamond")); + } + // ! ways to not coast // // * 1. try zero joystick input: new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 0.0, 0.0, false), m_robotSwerveDrive); // * 2. try opposite joystick input: new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, false), m_robotSwerveDrive); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 43eb4f8..c21c45c 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -75,12 +75,7 @@ public class SwerveDrive extends SubsystemBase { // VecBuilder.fill(Units.degreesToRadians(1)), // TODO: tune // VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1))); // TODO: tune - m_odometry = new SwerveDriveOdometry( - m_kinematics, - getRegGyro(),//m_gyro.getRotation2d(), - new Pose2d()); // TODO: tune - - // m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d()); + m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d()); m_gyro.reset(); SmartDashboard.putData("Field", m_field); @@ -192,9 +187,9 @@ public class SwerveDrive extends SubsystemBase { // chassis speeds // TODO: find the actual max velocity in m/s of the robot in fast mode to have accurate chassis speeds - SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond); - SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond); - SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond); + // SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond); + // SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond); + // SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond); } /** @@ -246,11 +241,11 @@ public class SwerveDrive extends SubsystemBase { SmartDashboard.putNumber("AUTO ACTUAL GYRO", actual.getDegrees()); SmartDashboard.putNumber("AUTO DWI GYRO", actual.getDegrees()); - m_odometry.update( actualDWI,//new Rotation2d((2 * Math.PI) - getRegGyro().getRadians()), - modules[0].getState(), - modules[1].getState(), - modules[2].getState(), - modules[3].getState()); + m_odometry.update( m_gyro.getRotation2d(),//new Rotation2d((2 * Math.PI) - getRegGyro().getRadians()), + modules[0].getState(), + modules[1].getState(), + modules[2].getState(), + modules[3].getState()); } From a6bfaa1b30f1eee05c1eba4d0671449fb32e3ab0 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 2 Apr 2022 15:01:09 -0600 Subject: [PATCH 4/4] change --- .../java/frc4388/robot/RobotContainer.java | 23 +++++++------------ 1 file changed, 8 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index f8eb76d..79e106a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -504,15 +504,6 @@ public class RobotContainer { */ public Command getAutonomousCommand() { - // ! PathPlanner Testing - if (!this.currentClimberMode.equals(null)) { - return new SequentialCommandGroup(buildAuto(1.0, 1.0, "Drive Forward")); - } - - if (!this.currentClimberMode.equals(null)) { - return new SequentialCommandGroup(buildAuto(1.0, 1.0, "Diamond")); - } - // ! ways to not coast // // * 1. try zero joystick input: new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 0.0, 0.0, false), m_robotSwerveDrive); // * 2. try opposite joystick input: new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, false), m_robotSwerveDrive); @@ -541,12 +532,12 @@ public class RobotContainer { )); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead. // ! DRIVE BACKWARDS AND SHOOT (HOPEFULLY) - return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving - new DriveWithInputForTime(m_robotSwerveDrive, new double[] {1.0, 0.0, 0.0, 0.0}, (5.0 * 12) / distancePerSecond),//0.269), // * go backwards three feet - new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive), // * brake + // return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving + // new DriveWithInputForTime(m_robotSwerveDrive, new double[] {1.0, 0.0, 0.0, 0.0}, (5.0 * 12) / distancePerSecond),//0.269), // * go backwards three feet + // new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive), // * brake - weirdAutoShootingGroup, // * shoot - new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.5)); // * stop running storage + // weirdAutoShootingGroup, // * shoot + // new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.5)); // * stop running storage // ! TWO BALL AUTO (HOPEFULLY) // return new SequentialCommandGroup( new ExtenderIntakeGroup(m_robotIntake, m_robotExtender), // * extend out, in preparation of running intake @@ -619,7 +610,9 @@ public class RobotContainer { // new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 1.0)) // ); - + // ! PathPlanner Testing + return new SequentialCommandGroup(buildAuto(1.0, 1.0, "Move Forward")); + // return new SequentialCommandGroup(buildAuto(1.0, 1.0, "Diamond")); } public void switchControlMode() {