Setup Swerve and add Deadbanded Xbox

src/main/java/frc4388/robot/Constants.java

@@ -148,5 +148,7 @@ public final class Constants {
     public static final class OIConstants {
         public static final int XBOX_DRIVER_ID = 0;
         public static final int XBOX_OPERATOR_ID = 1;
+        public static final double LEFT_AXIS_DEADBAND = 0.1;
+
     }
 }

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

@@ -16,7 +16,9 @@ import frc4388.robot.Constants.*;
 import frc4388.robot.commands.Swerve.JoystickPlayback;
 import frc4388.robot.commands.Swerve.JoystickRecorder;
 import frc4388.robot.subsystems.LED;
+import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.utility.LEDPatterns;
+import frc4388.utility.controller.DeadbandedXboxController;
 import frc4388.utility.controller.IHandController;
 import frc4388.utility.controller.XboxController;
 
@@ -34,9 +36,16 @@ public class RobotContainer {
     /* Subsystems */
     private final LED m_robotLED = new LED(m_robotMap.LEDController);
 
+    public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront,
+                                                                  m_robotMap.rightFront,
+                                                                  m_robotMap.leftBack,
+                                                                  m_robotMap.rightBack,
+                                                                  m_robotMap.gyro);
+    
+
     /* Controllers */
-    private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
+    private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
-    private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
+    private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
 
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -74,9 +83,9 @@ public class RobotContainer {
 
         /* Operator Buttons */
         // activates "Lit Mode"
-        new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
+        // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
-                .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
+        //         .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
-                .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
+        //         .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
     }
 
     /**
@@ -92,28 +101,11 @@ public class RobotContainer {
     /**
      * Add your docs here.
      */
-    public IHandController getDriverController() {
+    public DeadbandedXboxController getDeadbandedDriverController() {
-        return m_driverXbox;
+        return this.m_driverXbox;
     }
 
-    /**
+    public DeadbandedXboxController getDeadbandedOperatorController() {
-     * Add your docs here.
+        return this.m_operatorXbox;
-     */
-    public IHandController getOperatorController() {
-        return m_operatorXbox;
-    }
-
-    /**
-     * Add your docs here.
-     */
-    public Joystick getOperatorJoystick() {
-        return m_operatorXbox.getJoyStick();
-    }
-
-    /**
-     * Add your docs here.
-     */
-    public Joystick getDriverJoystick() {
-        return m_driverXbox.getJoyStick();
     }
 }

src/main/java/frc4388/robot/RobotMap.java

@@ -10,11 +10,14 @@ package frc4388.robot;
 import com.ctre.phoenix.motorcontrol.InvertType;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+import com.ctre.phoenix.sensors.CANCoder;
 import com.ctre.phoenix.sensors.PigeonIMU;
+import com.ctre.phoenix.sensors.WPI_Pigeon2;
 
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
-import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import frc4388.robot.Constants.LEDConstants;
+import frc4388.robot.Constants.SwerveDriveConstants;
+import frc4388.robot.subsystems.SwerveModule;
 import frc4388.utility.RobotGyro;
 
 /**
@@ -22,6 +25,13 @@ import frc4388.utility.RobotGyro;
  * testing and modularization.
  */
 public class RobotMap {
+    private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14);
+    public RobotGyro gyro = new RobotGyro(m_pigeon2);
+
+    public SwerveModule leftFront;
+    public SwerveModule rightFront;
+    public SwerveModule leftBack;
+    public SwerveModule rightBack;
 
     public RobotMap() {
         configureLEDMotorControllers();
@@ -31,10 +41,56 @@ public class RobotMap {
     /* LED Subsystem */
     public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
 
-    void configureLEDMotorControllers() {
 
+    /* Swreve Drive Subsystem */
+    public final WPI_TalonFX leftFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID);
+    public final WPI_TalonFX leftFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID);
+    public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID);
+
+    public final WPI_TalonFX rightFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID);
+    public final WPI_TalonFX rightFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID);
+    public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID);
+        
+    public final WPI_TalonFX leftBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID);
+    public final WPI_TalonFX leftBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID);
+    public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID);
+
+    public final WPI_TalonFX rightBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID);
+    public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID);
+    public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID);
+
+    void configureLEDMotorControllers() {    
     }
 
     void configureDriveMotorControllers() {
+        // config factory default
+        leftFrontWheel.configFactoryDefault();
+        leftFrontSteer.configFactoryDefault();
+        
+        rightFrontWheel.configFactoryDefault();
+        rightFrontSteer.configFactoryDefault();
+        
+        leftBackWheel.configFactoryDefault();
+        leftBackSteer.configFactoryDefault();
+        
+        rightBackWheel.configFactoryDefault();
+        rightBackSteer.configFactoryDefault();
+
+        // set neutral mode
+        leftFrontWheel.setNeutralMode(NeutralMode.Brake);
+        rightFrontWheel.setNeutralMode(NeutralMode.Brake);
+        leftBackWheel.setNeutralMode(NeutralMode.Brake);
+        rightBackWheel.setNeutralMode(NeutralMode.Brake);
+
+        leftFrontSteer.setNeutralMode(NeutralMode.Brake);
+        rightFrontSteer.setNeutralMode(NeutralMode.Brake);
+        leftBackSteer.setNeutralMode(NeutralMode.Brake);
+        rightBackSteer.setNeutralMode(NeutralMode.Brake);
+
+        // initialize SwerveModules
+        this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, -181.230469);
+        this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, -270.615234);
+        this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, -240.029297);
+        this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, -40.869142);
     }
 }

src/main/java/frc4388/utility/controller/DeadbandedXboxController.java

@@ -0,0 +1,27 @@
+package frc4388.utility.controller;
+
+import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.XboxController;
+
+public class DeadbandedXboxController extends XboxController {
+  public DeadbandedXboxController(int port) { super(port); }
+  
+  @Override public double getLeftX() { return getLeft().getX(); }
+  @Override public double getLeftY() { return getLeft().getY(); }
+  @Override public double getRightX() { return getRight().getX(); }
+  @Override public double getRightY() { return getRight().getY(); }
+
+  public Translation2d getLeft() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()); }
+  public Translation2d getRight() { return skewToDeadzonedCircle(-super.getRightX(), super.getRightY()); }
+
+  public static Translation2d skewToDeadzonedCircle(double x, double y) {
+    Translation2d translation2d = new Translation2d(x, y);
+    double magnitude = translation2d.getNorm();
+
+    if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0);
+
+    return translation2d;
+  }
+}