Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-09 00:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

Add Drift correction

Browse Source
This commit is contained in:
C4llSiqn
2025-01-18 11:36:26 -07:00
parent a6a9d7d5c1
commit 453f56cf59
2 changed files with 220 additions and 191 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+13 -5
View File
@@ -84,7 +84,7 @@ public final class Constants {
public static final double[] GEARS = {SLOW_SPEED, FAST_SPEED, TURBO_SPEED}; public static final double[] GEARS = {SLOW_SPEED, FAST_SPEED, TURBO_SPEED};
public static final int STARTING_GEAR = 0; public static final int STARTING_GEAR = 0;
// dimensions // Dimensions
public static final double WIDTH = 18.5; // TODO: validate public static final double WIDTH = 18.5; // TODO: validate
public static final double HEIGHT = 18.5; // TODO: validate public static final double HEIGHT = 18.5; // TODO: validate
public static final double HALF_WIDTH = WIDTH / 2.d; public static final double HALF_WIDTH = WIDTH / 2.d;
@@ -101,11 +101,17 @@ public final class Constants {
public static final double MAX_SPEED_FEET_PER_SECOND = MAX_SPEED_MEETERS_PER_SEC * 3.28084; public static final double MAX_SPEED_FEET_PER_SECOND = MAX_SPEED_MEETERS_PER_SEC * 3.28084;
public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI;
// Operation
public static final double FORWARD_OFFSET = 90; // 0, 90, 180, 270 public static final double FORWARD_OFFSET = 90; // 0, 90, 180, 270
public static final boolean DRIFT_CORRECTION_ENABLED = true;
public static final boolean INVERT_X = true;
public static final boolean INVERT_Y = false;
public static final boolean INVERT_ROTATION = false;
// public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0); // public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0);
private static final class ModuleSpecificConstants { //2025 /* private static final class ModuleSpecificConstants { //2025
//Front Left //Front Left
private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.229736328125+.25); private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.229736328125+.25);
private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false;
@@ -137,9 +143,9 @@ public final class Constants {
private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; private static final boolean BACK_RIGHT_ENCODER_INVERTED = false;
private static final Distance BACK_RIGHT_XPOS = Inches.of(HALF_WIDTH); private static final Distance BACK_RIGHT_XPOS = Inches.of(HALF_WIDTH);
private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
} } */
/* private static final class ModuleSpecificConstants { // 2024 private static final class ModuleSpecificConstants { // 2024
//Front Left //Front Left
private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.36328125); private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.36328125);
private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false;
@@ -171,7 +177,7 @@ public final class Constants {
private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; private static final boolean BACK_RIGHT_ENCODER_INVERTED = false;
private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_WIDTH); private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_WIDTH);
private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
} */ }
public static final class IDs { public static final class IDs {
public static final CanDevice RIGHT_FRONT_WHEEL = new CanDevice("RIGHT_FRONT_WHEEL", 4); public static final CanDevice RIGHT_FRONT_WHEEL = new CanDevice("RIGHT_FRONT_WHEEL", 4);
@@ -219,6 +225,8 @@ public final class Constants {
public static final Slot0Configs PREPROVIDED_DRIVE_GAINS = new Slot0Configs() public static final Slot0Configs PREPROVIDED_DRIVE_GAINS = new Slot0Configs()
.withKP(0.1).withKI(0).withKD(0) .withKP(0.1).withKI(0).withKD(0)
.withKS(0).withKV(0.124); .withKS(0).withKV(0.124);
public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1);
} }
public static final class AutoConstants { public static final class AutoConstants {
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+197 -176
View File
@@ -49,7 +49,8 @@ public class SwerveDrive extends Subsystem {
public double speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * SwerveDriveConstants.GEARS[gear_index]; public double speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * SwerveDriveConstants.GEARS[gear_index];
public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED; public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED;
public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to 25% public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to
// 25%
public double rotTarget = 0.0; public double rotTarget = 0.0;
public Rotation2d orientRotTarget = new Rotation2d(); public Rotation2d orientRotTarget = new Rotation2d();
@@ -57,278 +58,298 @@ public class SwerveDrive extends Subsystem {
/** Creates a new SwerveDrive. */ /** Creates a new SwerveDrive. */
public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain, Vision vision) { public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain, Vision vision) {
// public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain) { // public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder>
super(); // swerveDriveTrain) {
super();
this.swerveDriveTrain = swerveDriveTrain; this.swerveDriveTrain = swerveDriveTrain;
this.vision = vision; this.vision = vision;
RobotConfig config; RobotConfig config;
try{ try {
config = RobotConfig.fromGUISettings(); config = RobotConfig.fromGUISettings();
} catch (Exception e) { } catch (Exception e) {
// Handle exception as needed // Handle exception as needed
config = null; config = null;
} }
// DoubleSupplier a = () -> 1.d; // DoubleSupplier a = () -> 1.d;
AutoBuilder.configure( AutoBuilder.configure(
() -> { () -> {
var pose = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()); var pose = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d());
// pose = new Pose2d(pose.getX(), pose.getY(), pose.getRotation().times(-1)); // pose = new Pose2d(pose.getX(), pose.getY(), pose.getRotation().times(-1));
return pose;//getRotation().times(-1) return pose;// getRotation().times(-1)
}, // Robot pose supplier }, // Robot pose supplier
swerveDriveTrain::resetPose, // Method to reset odometry (will be called if your auto has a starting pose) swerveDriveTrain::resetPose, // Method to reset odometry (will be called if your auto has a starting
() -> swerveDriveTrain.getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE // pose)
(speeds, feedforwards) -> swerveDriveTrain.setControl(new SwerveRequest.ApplyRobotSpeeds() () -> swerveDriveTrain.getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
.withSpeeds(speeds) (speeds, feedforwards) -> swerveDriveTrain.setControl(new SwerveRequest.ApplyRobotSpeeds()
), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards .withSpeeds(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds.
new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains // Also optionally outputs individual module feedforwards
new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for
new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants // holonomic drive trains
), new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
config, // The robot configuration new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants
() -> { ),
// Boolean supplier that controls when the path will be mirrored for the red alliance config, // The robot configuration
// This will flip the path being followed to the red side of the field. () -> {
// THE ORIGIN WILL REMAIN ON THE BLUE SIDE // Boolean supplier that controls when the path will be mirrored for the red
// alliance
// This will flip the path being followed to the red side of the field.
// THE ORIGIN WILL REMAIN ON THE BLUE SIDE
var alliance = DriverStation.getAlliance(); var alliance = DriverStation.getAlliance();
if (alliance.isPresent()) { if (alliance.isPresent()) {
return alliance.get() == DriverStation.Alliance.Red; return alliance.get() == DriverStation.Alliance.Red;
} }
return false; return false;
}, },
this // Reference to this subsystem to set requirements this // Reference to this subsystem to set requirements
); );
} }
// public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){ // public void oneModuleTest(SwerveModule module, Translation2d leftStick,
// // double ang = Math.atan2(rightStick.getY(), rightStick.getX()); // Translation2d rightStick){
// // rightStick.getAngle() // // double ang = Math.atan2(rightStick.getY(), rightStick.getX());
// double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) + Math.pow(leftStick.getY(), 2)); // // rightStick.getAngle()
// // System.out.println(ang); // double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) +
// // module.go(ang); // Math.pow(leftStick.getY(), 2));
// // Rotation2d rot = Rotation2d.fromRadians(ang); // // System.out.println(ang);
// Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY()); // // module.go(ang);
// SwerveModuleState state = new SwerveModuleState(speed, rot); // // Rotation2d rot = Rotation2d.fromRadians(ang);
// module.setDesiredState(state); // Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY());
// SwerveModuleState state = new SwerveModuleState(speed, rot);
// module.setDesiredState(state);
// } // }
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
stopModules(); // stop the swerve stopModules(); // stop the swerve
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) //if no imput if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput
return; // don't bother doing swerve drive math and return early. return; // don't bother doing swerve drive math and return early.
leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
if (SwerveDriveConstants.INVERT_X) leftStick = new Translation2d(-leftStick.getX(), leftStick.getY());
if (SwerveDriveConstants.INVERT_Y) leftStick = new Translation2d(leftStick.getX(), -leftStick.getY());
if (SwerveDriveConstants.INVERT_ROTATION) rightStick.times(-1);
stopped = false;
if (fieldRelative) {
// ! drift correction
if (rightStick.getNorm() > 0.05 || !SwerveDriveConstants.DRIFT_CORRECTION_ENABLED) {
rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees();
swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
.withVelocityX(leftStick.getX() * speedAdjust)
.withVelocityY(leftStick.getY() * speedAdjust)
.withRotationalRate(rightStick.getX() * rotSpeedAdjust));
SmartDashboard.putBoolean("drift correction", false);
} else {
var ctrl = new SwerveRequest.FieldCentricFacingAngle()
.withVelocityX(leftStick.getX() * speedAdjust)
.withVelocityY(leftStick.getY() * speedAdjust)
.withTargetDirection(Rotation2d.fromDegrees(rotTarget));
ctrl.HeadingController.setPID(
SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP,
SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI,
SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD
);
swerveDriveTrain.setControl(ctrl);
SmartDashboard.putBoolean("drift correction", true);
}
if (fieldRelative) { // // SmartDashboard.putBoolean("drift correction", true);
swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
.withVelocityX(leftStick.getX()*speedAdjust)
.withVelocityY(-leftStick.getY()*speedAdjust)
.withRotationalRate(rightStick.getX()*rotSpeedAdjust)
);
// double rot = 0;
// ! drift correction // // rot = ((rotTarget - gyro.getAngle()) / 360) *
// dependant on if the new odomitry system acounts for rotational drift, this may not be needed. // SwerveDriveConstants.ROT_CORRECTION_SPEED;
// if (rightStick.getNorm() > 0.05) {
// rotTarget = swerveDriveTrain.getRotation3d().toRotation2d().getDegrees();
// swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
// .withVelocityX(leftStick.getX()*speedAdjust)
// .withVelocityY(leftStick.getY()*speedAdjust)
// .withRotationalRate(rightStick.getY()*rotSpeedAdjust)
// );
// // SmartDashboard.putBoolean("drift correction", false); // }
// stopped = false;
// } else if(leftStick.getNorm() > 0.05) {
// if (!stopped) {
// stopModules();
// stopped = true;
// }
// // SmartDashboard.putBoolean("drift correction", true); // // Use the left joystick to set speed. Apply a cubic curve and the set max
// speed.
// Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
// // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00),
// Math.pow(speed.getY(), 3.00));
// // rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; // // Convert field-relative speeds to robot-relative speeds.
// // chassisSpeeds = chassisSpeeds.
// } // chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 *
// speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction,
// // Use the left joystick to set speed. Apply a cubic curve and the set max speed. // gyro.getRotation2d().times(-1));
// Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); } else { // Create robot-relative speeds.
// // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
.withVelocityX(leftStick.getX() * speedAdjust)
// // Convert field-relative speeds to robot-relative speeds. .withVelocityY(-leftStick.getY() * speedAdjust)
// // chassisSpeeds = chassisSpeeds. .withRotationalRate(rightStick.getX() * rotSpeedAdjust));
// chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1)); }
} else { // Create robot-relative speeds.
swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
.withVelocityX(leftStick.getX()*speedAdjust)
.withVelocityY(-leftStick.getY()*speedAdjust)
.withRotationalRate(rightStick.getX()*rotSpeedAdjust)
);
}
} }
public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical reason to have a robot relitive version of this, and no pre provided version public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: // reason to have a robot
stopModules(); // stop the swerve // relitive version of
// this, and no pre
// provided version
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve
// drive is still going:
stopModules(); // stop the swerve
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) //if no imput if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput
return; // don't bother doing swerve drive math and return early. return; // don't bother doing swerve drive math and return early.
leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
.withVelocityX(leftStick.getX()*speedAdjust) .withVelocityX(leftStick.getX() * speedAdjust)
.withVelocityY(leftStick.getY()*speedAdjust) .withVelocityY(leftStick.getY() * speedAdjust)
.withTargetDirection(rightStick.getAngle()) .withTargetDirection(rightStick.getAngle()));
);
} }
public boolean rotateToTarget(double angle) { public boolean rotateToTarget(double angle) {
swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
.withVelocityX(0) .withVelocityX(0)
.withVelocityY(0) .withVelocityY(0)
.withTargetDirection(Rotation2d.fromDegrees(angle)) .withTargetDirection(Rotation2d.fromDegrees(angle)));
);
if (Math.abs(angle - getGyroAngle()) < 5.0) { if (Math.abs(angle - getGyroAngle()) < 5.0) {
return true; return true;
} }
return false; return false;
} }
public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) { public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) {
// if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: // if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the
// stopModules(); // stop the swerve // swerve drive is still going:
// stopModules(); // stop the swerve
// if (leftStick.getNorm() < 0.05) //if no imput // if (leftStick.getNorm() < 0.05) //if no imput
// return; // don't bother doing swerve drive math and return early. // return; // don't bother doing swerve drive math and return early.
leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
.withVelocityX(leftStick.getX()*-speedAdjust) .withVelocityX(leftStick.getX() * -speedAdjust)
.withVelocityY(leftStick.getY()*speedAdjust) .withVelocityY(leftStick.getY() * speedAdjust)
.withTargetDirection(rot) .withTargetDirection(rot));
); // double
// double
} }
public double getGyroAngle() { public double getGyroAngle() {
return swerveDriveTrain.getRotation3d().getAngle(); return swerveDriveTrain.getRotation3d().getAngle();
} }
public void resetGyro() { public void resetGyro() {
swerveDriveTrain.tareEverything(); swerveDriveTrain.tareEverything();
} }
public void stopModules() { public void stopModules() {
swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); stopped = true;
} swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake());
}
@Override @Override
public void periodic() { public void periodic() {
// This method will be called once per scheduler run\ // This method will be called once per scheduler run\
SmartDashboard.putNumber("Gyro", getGyroAngle()); SmartDashboard.putNumber("Gyro", getGyroAngle());
SmartDashboard.putNumber("RotTartget", rotTarget); SmartDashboard.putNumber("RotTartget", rotTarget);
double time = Vision.getTime(); double time = Vision.getTime();
vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(time)); vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(time));
if(vision.isTag()){ if (vision.isTag()) {
swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time); swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time);
} }
// if(e.isPresent()) // if(e.isPresent())
} }
private void reset_index() { private void reset_index() {
gear_index = SwerveDriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the robot start in?) gear_index = SwerveDriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the
// robot start in?)
} }
public void shiftDown() { public void shiftDown() {
if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length)
int i = gear_index - 1; reset_index(); // If outof bounds: reset index
if (i == -1) i = 0; int i = gear_index - 1;
setPercentOutput(SwerveDriveConstants.GEARS[i]); if (i == -1)
gear_index = i; i = 0;
setPercentOutput(SwerveDriveConstants.GEARS[i]);
gear_index = i;
} }
public void shiftUp() { public void shiftUp() {
if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length)
int i = gear_index + 1; reset_index(); // If outof bounds: reset index
if (i == SwerveDriveConstants.GEARS.length) i = SwerveDriveConstants.GEARS.length - 1; int i = gear_index + 1;
setPercentOutput(SwerveDriveConstants.GEARS[i]); if (i == SwerveDriveConstants.GEARS.length)
gear_index = i; i = SwerveDriveConstants.GEARS.length - 1;
setPercentOutput(SwerveDriveConstants.GEARS[i]);
gear_index = i;
} }
public void setPercentOutput(double speed) { public void setPercentOutput(double speed) {
speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed; speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
gear_index = -1; gear_index = -1;
} }
public void setToSlow() { public void setToSlow() {
setPercentOutput(SwerveDriveConstants.SLOW_SPEED); setPercentOutput(SwerveDriveConstants.SLOW_SPEED);
gear_index = 0; gear_index = 0;
} }
public void setToFast() { public void setToFast() {
setPercentOutput(SwerveDriveConstants.FAST_SPEED); setPercentOutput(SwerveDriveConstants.FAST_SPEED);
gear_index = 1; gear_index = 1;
} }
public void setToTurbo() { public void setToTurbo() {
setPercentOutput(SwerveDriveConstants.TURBO_SPEED); setPercentOutput(SwerveDriveConstants.TURBO_SPEED);
gear_index = 2; gear_index = 2;
} }
public void shiftUpRot() { public void shiftUpRot() {
rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED; rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED;
} }
public void shiftDownRot() { public void shiftDownRot() {
rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED; rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
} }
@Override @Override
public String getSubsystemName() { public String getSubsystemName() {
return "Swerve Drive Controller"; return "Swerve Drive Controller";
} }
ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems") ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
.getLayout(getSubsystemName(), BuiltInLayouts.kList) .getLayout(getSubsystemName(), BuiltInLayouts.kList)
.withSize(2, 2); .withSize(2, 2);
GenericEntry sbGyro = subsystemLayout GenericEntry sbGyro = subsystemLayout
.add("Gyro angle", 0) .add("Gyro angle", 0)
.withWidget(BuiltInWidgets.kGyro) .withWidget(BuiltInWidgets.kGyro)
.getEntry(); .getEntry();
GenericEntry sbShiftState = subsystemLayout GenericEntry sbShiftState = subsystemLayout
.add("Shift State", 0) .add("Shift State", 0)
.withWidget(BuiltInWidgets.kNumberBar) .withWidget(BuiltInWidgets.kNumberBar)
.getEntry(); .getEntry();
@Override @Override
public void queryStatus() { public void queryStatus() {
sbGyro.setDouble(getGyroAngle()); sbGyro.setDouble(getGyroAngle());
sbShiftState.setDouble(this.speedAdjust); sbShiftState.setDouble(this.speedAdjust);
//TODO: Add more status things // TODO: Add more status things
} }
@Override @Override
public Status diagnosticStatus() { public Status diagnosticStatus() {
Status status = new Status(); Status status = new Status();
status.addReport(ReportLevel.ERROR, "Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there."); status.addReport(ReportLevel.ERROR,
"Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there.");
return status; return status;
} }
} }