Merge branch 'cleanup' of https://github.com/Team4388/2022NoWayHome into cleanup

src/main/java/frc4388/robot/RobotContainer.java

@@ -458,6 +458,7 @@ public class RobotContainer {
     maxAccel = Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.PATH_MAX_ACCELERATION);
 
     ArrayList<Command> commands = new ArrayList<Command>();
+    commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
 
     PIDController xController = SwerveDriveConstants.X_CONTROLLER;
     PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
@@ -531,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
+    // 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 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
+    //                                    new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive), // * brake
                                        
-                                       weirdAutoShootingGroup, // * shoot
+    //                                    weirdAutoShootingGroup, // * shoot
-                                       new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.5)); // * stop running storage
+    //                                    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
@@ -609,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() {

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_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d());
-        m_kinematics,
-        getRegGyro(),//m_gyro.getRotation2d(),
-        new Pose2d()); // TODO: tune
-
-    // 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: X", chassisSpeeds.vxMetersPerSecond);
-    SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond);
+    // SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond);
-    SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond);
+    // 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()),
+    m_odometry.update( m_gyro.getRotation2d(),//new Rotation2d((2 * Math.PI) - getRegGyro().getRadians()),
-                            modules[0].getState(),
+                       modules[0].getState(),
-                            modules[1].getState(),
+                       modules[1].getState(),
-                            modules[2].getState(),
+                       modules[2].getState(),
-                            modules[3].getState());
+                       modules[3].getState());
   }
   
   

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();