From e12199ccd9d86e51a734c94902120993ce7d5502 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 14 Jan 2023 11:42:03 -0700 Subject: [PATCH] SwerveDrive class done --- src/main/java/frc4388/robot/Constants.java | 37 +++++---- .../frc4388/robot/subsystems/SwerveDrive.java | 78 +++++++++++++++++++ .../robot/subsystems/SwerveModule.java | 25 +++++- 3 files changed, 123 insertions(+), 17 deletions(-) create mode 100644 src/main/java/frc4388/robot/subsystems/SwerveDrive.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7f5b739..ba1b8ea 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -21,22 +21,22 @@ import frc4388.utility.LEDPatterns; public final class Constants { public static final class SwerveDriveConstants { public static final class IDs { - public static final int DRIVE_PIGEON_ID = -1; + public static final int DRIVE_PIGEON_ID = -1; // TODO: find actual ID - public static final int LEFT_FRONT_WHEEL_ID = -1; - public static final int RIGHT_FRONT_WHEEL_ID = -1; - public static final int LEFT_BACK_WHEEL_ID = -1; - public static final int RIGHT_BACK_STEER_ID = -1; - - public static final int LEFT_FRONT_STEER_ID = -1; - public static final int RIGHT_FRONT_STEER_ID = -1; - public static final int LEFT_BACK_STEER_ID = -1; - public static final int RIGHT_BACK_WHEEL_ID = -1; - - public static final int LEFT_FRONT_ENCODER_ID = -1; - public static final int RIGHT_FRONT_ENCODER_ID = -1; - public static final int LEFT_BACK_ENCODER_ID = -1; - public static final int RIGHT_BACK_ENCODER_ID = -1; + 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_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 { @@ -47,10 +47,15 @@ public final class Constants { 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 double MAX_SPEED_FEET_PER_SECOND = -1; // TODO: find the actual value - public static final double MAX_SPEED_FEET_PER_SECOND = 16; // TODO: find the actual value + 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; public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java new file mode 100644 index 0000000..025e5f9 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -0,0 +1,78 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.subsystems; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.trajectory.constraint.SwerveDriveKinematicsConstraint; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.utility.RobotGyro; + +public class SwerveDrive extends SubsystemBase { + + private SwerveModule leftFront; + private SwerveModule rightFront; + private SwerveModule leftBack; + private SwerveModule rightBack; + + private SwerveModule[] modules; + + private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); + private Translation2d rightFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); + private Translation2d leftBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); + private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); + + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); + + private RobotGyro gyro; + + /** Creates a new SwerveDrive. */ + public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) { + this.leftFront = leftFront; + this.rightFront = rightFront; + this.leftBack = leftBack; + this.rightBack = rightBack; + + this.modules = new SwerveModule[] {leftFront, rightFront, leftBack, rightBack}; + + this.gyro = gyro; + } + + public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { + double xSpeedMetersPerSecond = xSpeed * SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND; + double ySpeedMetersPerSecond = ySpeed * SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND; + + SwerveModuleState[] states = kinematics.toSwerveModuleStates( + fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d()) + : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot) + ); + + SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.metersToFeet(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + setModuleStates(states); + } + + /** + * 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) { + for (int i = 0; i < desiredStates.length; i++) { + SwerveModule module = modules[i]; + SwerveModuleState state = desiredStates[i]; + module.setDesiredState(state); + } + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index ee495fe..ea9bc9b 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -26,7 +26,6 @@ public class SwerveModule extends SubsystemBase { public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS; - /** Creates a new SwerveModule. */ public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) { this.driveMotor = driveMotor; @@ -49,6 +48,30 @@ public class SwerveModule extends SubsystemBase { canCoder.configAllSettings(canCoderConfig); } + /** + * Get the drive motor of the SwerveModule + * @return the drive motor of the SwerveModule + */ + public WPI_TalonFX getDriveMotor() { + return this.driveMotor; + } + + /** + * Get the angle motor of the SwerveModule + * @return the angle motor of the SwerveModule + */ + public WPI_TalonFX getAngleMotor() { + return this.angleMotor; + } + + /** + * Get the CANcoder of the SwerveModule + * @return the CANcoder of the SwerveModule + */ + public CANCoder getEncoder() { + return this.canCoder; + } + /** * Get the angle of a SwerveModule as a Rotation2d * @return the angle of a SwerveModule as a Rotation2d