From 72183b7b2aa418cbb4acd76741192709710d4a35 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 14 Jan 2023 12:58:18 -0700 Subject: [PATCH] SwerveDrive RobotMap done --- src/main/java/frc4388/robot/Constants.java | 15 ++- src/main/java/frc4388/robot/RobotMap.java | 98 +++++++++++++++++-- .../frc4388/robot/subsystems/SwerveDrive.java | 1 - 3 files changed, 102 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index ba1b8ea..403ea2c 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -50,14 +50,27 @@ public final class Constants { public static final double JOYSTICK_TO_METERS_PER_SECOND = 5.0; } + public static final class Configurations { + public static final double OPEN_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value + public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value + public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value + + public static final double LEFT_FRONT_ENCODER_OFFSET = -1.0; // TODO: find the actual value + public static final double RIGHT_FRONT_ENCODER_OFFSET = -1.0; // TODO: find the actual value + public static final double LEFT_BACK_ENCODER_OFFSET = -1.0; // TODO: find the actual value + public static final double RIGHT_BACK_ENCODER_OFFSET = -1.0; // TODO: find the actual value + } + public static final double MAX_SPEED_FEET_PER_SECOND = -1; // TODO: find the actual value + // dimensions public static final double WIDTH = -1; // TODO: find the actual value public static final double HEIGHT = -1; // TODO: find the actual value public static final double HALF_WIDTH = WIDTH / 2.d; public static final double HALF_HEIGHT = HEIGHT / 2.d; - public static final int SWERVE_TIMEOUT_MS = 30; + // misc + public static final int TIMEOUT_MS = 30; public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 5f17853..8943fc6 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -10,11 +10,13 @@ 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 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.utility.RobotGyro; /** @@ -23,18 +25,94 @@ import frc4388.utility.RobotGyro; */ public class RobotMap { - public RobotMap() { - configureLEDMotorControllers(); - configureDriveMotorControllers(); - } + public RobotMap() { + configureLEDMotorControllers(); + configureDriveMotors(); + } - /* LED Subsystem */ - public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); + /* LED Subsystem */ + public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); - void configureLEDMotorControllers() { + void configureLEDMotorControllers() { - } + } - void configureDriveMotorControllers() { - } + // swerve 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 configureDriveMotors() { + // config factory default + leftFrontWheel.configFactoryDefault(); + leftFrontSteer.configFactoryDefault(); + + rightFrontWheel.configFactoryDefault(); + rightFrontSteer.configFactoryDefault(); + + leftBackWheel.configFactoryDefault(); + leftBackSteer.configFactoryDefault(); + + rightBackWheel.configFactoryDefault(); + rightBackSteer.configFactoryDefault(); + + // config open loop ramp + leftFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + leftFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + rightFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + rightFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + leftBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + leftBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + rightBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + rightBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + // config closed loop ramp + leftFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + leftFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + rightFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + rightFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + leftBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + leftBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + rightBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + rightBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + // config neutral deadband + leftFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + leftFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + + rightFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + rightFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + + leftBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + leftBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + + rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + + // config magnet offset + leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.LEFT_FRONT_ENCODER_OFFSET); + rightFrontEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.RIGHT_FRONT_ENCODER_OFFSET); + leftBackEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.LEFT_BACK_ENCODER_OFFSET); + rightBackEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.RIGHT_BACK_ENCODER_OFFSET); + + } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 025e5f9..0f32721 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -60,7 +60,6 @@ public class SwerveDrive extends SubsystemBase { /** * Set each module of the swerve drive to the corresponding desired state. - * * @param desiredStates Array of module states to set. */ public void setModuleStates(SwerveModuleState[] desiredStates) {