From b1f0799f7e6cad9e9cda5208b0d590687224faf5 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 14 Jan 2023 13:20:04 -0700 Subject: [PATCH] finished RobotMap, started RobotContainer work --- src/main/java/frc4388/robot/Constants.java | 120 +++++++++--------- .../java/frc4388/robot/RobotContainer.java | 3 + src/main/java/frc4388/robot/RobotMap.java | 18 +++ .../frc4388/robot/subsystems/SwerveDrive.java | 6 +- 4 files changed, 86 insertions(+), 61 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 403ea2c..4b1f668 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -19,69 +19,71 @@ import frc4388.utility.LEDPatterns; * constants are needed, to reduce verbosity. */ public final class Constants { - public static final class SwerveDriveConstants { - public static final class IDs { - public static final int DRIVE_PIGEON_ID = -1; // TODO: find actual ID + public static final class SwerveDriveConstants { + public static final class IDs { + public static final int LEFT_FRONT_WHEEL_ID = -1; // TODO: find actual ID + public static final int RIGHT_FRONT_WHEEL_ID = -1; // TODO: find actual ID + public static final int LEFT_BACK_WHEEL_ID = -1; // TODO: find actual ID + public static final int RIGHT_BACK_STEER_ID = -1; // TODO: find actual ID - public static final int LEFT_FRONT_WHEEL_ID = -1; // TODO: find actual ID - public static final int RIGHT_FRONT_WHEEL_ID = -1; // TODO: find actual ID - public static final int LEFT_BACK_WHEEL_ID = -1; // TODO: find actual ID - public static final int RIGHT_BACK_STEER_ID = -1; // TODO: find actual ID + public static final int LEFT_FRONT_STEER_ID = -1; // TODO: find actual ID + public static final int RIGHT_FRONT_STEER_ID = -1; // TODO: find actual ID + public static final int LEFT_BACK_STEER_ID = -1; // TODO: find actual ID + public static final int RIGHT_BACK_WHEEL_ID = -1; // TODO: find actual ID - public static final int LEFT_FRONT_STEER_ID = -1; // TODO: find actual ID - public static final int RIGHT_FRONT_STEER_ID = -1; // TODO: find actual ID - public static final int LEFT_BACK_STEER_ID = -1; // TODO: find actual ID - public static final int RIGHT_BACK_WHEEL_ID = -1; // TODO: find actual ID - - public static final int LEFT_FRONT_ENCODER_ID = -1; // TODO: find actual ID - public static final int RIGHT_FRONT_ENCODER_ID = -1; // TODO: find actual ID - public static final int LEFT_BACK_ENCODER_ID = -1; // TODO: find actual ID - public static final int RIGHT_BACK_ENCODER_ID = -1; // TODO: find actual ID - } - - public static final class PIDConstants { - public static final int SWERVE_SLOT_IDX = 0; - public static final int SWERVE_PID_LOOP_IDX = 1; - public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0); - } - - public static final class Conversions { - public static final int CANCODER_TICKS_PER_ROTATION = 4096; - 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; - - // misc - public static final int TIMEOUT_MS = 30; - public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; + public static final int LEFT_FRONT_ENCODER_ID = -1; // TODO: find actual ID + public static final int RIGHT_FRONT_ENCODER_ID = -1; // TODO: find actual ID + public static final int LEFT_BACK_ENCODER_ID = -1; // TODO: find actual ID + public static final int RIGHT_BACK_ENCODER_ID = -1; // TODO: find actual ID } + + public static final class PIDConstants { + public static final int SWERVE_SLOT_IDX = 0; + public static final int SWERVE_PID_LOOP_IDX = 1; + public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0); + } + + public static final class Conversions { + public static final int CANCODER_TICKS_PER_ROTATION = 4096; + 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; + + // misc + public static final int TIMEOUT_MS = 30; + public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; + } + + public static final class GyroConstants { + public static final int ID = -1; // TODO: find actual ID + } - public static final class LEDConstants { - public static final int LED_SPARK_ID = 0; + public static final class LEDConstants { + public static final int LED_SPARK_ID = 0; - public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES; - } + public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES; + } - public static final class OIConstants { - public static final int XBOX_DRIVER_ID = 0; - public static final int XBOX_OPERATOR_ID = 1; - } + public static final class OIConstants { + public static final int XBOX_DRIVER_ID = 0; + public static final int XBOX_OPERATOR_ID = 1; + } } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 99f5cb1..02cd2b3 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.LEDPatterns; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; @@ -30,7 +31,9 @@ public class RobotContainer { private final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ + private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack, m_robotMap.gyro); private final LED m_robotLED = new LED(m_robotMap.LEDController); + /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 8943fc6..308a377 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -12,11 +12,14 @@ 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.GyroConstants; import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.robot.subsystems.SwerveModule; import frc4388.utility.RobotGyro; /** @@ -25,9 +28,18 @@ import frc4388.utility.RobotGyro; */ public class RobotMap { + public SwerveModule leftFront; + public SwerveModule rightFront; + public SwerveModule leftBack; + public SwerveModule rightBack; + + public WPI_Pigeon2 gyro; + public RobotMap() { configureLEDMotorControllers(); configureDriveMotors(); + + gyro = new WPI_Pigeon2(GyroConstants.ID); } /* LED Subsystem */ @@ -114,5 +126,11 @@ public class RobotMap { leftBackEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.LEFT_BACK_ENCODER_OFFSET); rightBackEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.RIGHT_BACK_ENCODER_OFFSET); + // initialize SwerveModules + this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.Configurations.LEFT_FRONT_ENCODER_OFFSET); + this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.Configurations.RIGHT_FRONT_ENCODER_OFFSET); + this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.Configurations.LEFT_BACK_ENCODER_OFFSET); + this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, 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 0f32721..f30e8ff 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -4,6 +4,8 @@ package frc4388.robot.subsystems; +import com.ctre.phoenix.sensors.WPI_Pigeon2; + import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -31,10 +33,10 @@ public class SwerveDrive extends SubsystemBase { private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); - private RobotGyro gyro; + private WPI_Pigeon2 gyro; /** Creates a new SwerveDrive. */ - public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) { + public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, WPI_Pigeon2 gyro) { this.leftFront = leftFront; this.rightFront = rightFront; this.leftBack = leftBack;