mirror of
https://github.com/Team4388/2023WayOfTheRobot.git
synced 2026-06-09 00:37:59 -06:00
SwerveDrive class done
This commit is contained in:
aarav18
@@ -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;
|
||||
|
||||
@@ -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
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user