From a1b28dabeea6d5758bbdc471defe37ad084bd6ef Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Mon, 8 Jan 2024 13:47:43 -0700 Subject: [PATCH] Setup Swerve and add Deadbanded Xbox --- src/main/java/frc4388/robot/Constants.java | 2 + .../java/frc4388/robot/RobotContainer.java | 44 ++++++-------- src/main/java/frc4388/robot/RobotMap.java | 60 ++++++++++++++++++- .../controller/DeadbandedXboxController.java | 27 +++++++++ 4 files changed, 105 insertions(+), 28 deletions(-) create mode 100644 src/main/java/frc4388/utility/controller/DeadbandedXboxController.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index a58a1d0..efc80a5 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/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; + } } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 23c1b0f..5934d2f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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 XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); + private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_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) - .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) - .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); + // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) + // .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) + // .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); } /** @@ -92,28 +101,11 @@ public class RobotContainer { /** * Add your docs here. */ - public IHandController getDriverController() { - return m_driverXbox; + public DeadbandedXboxController getDeadbandedDriverController() { + return this.m_driverXbox; } - /** - * Add your docs here. - */ - 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(); + public DeadbandedXboxController getDeadbandedOperatorController() { + return this.m_operatorXbox; } } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 5f17853..b7f03c4 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/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); } } diff --git a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java new file mode 100644 index 0000000..4577a2e --- /dev/null +++ b/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; + } +} \ No newline at end of file