From 453f56cf5963e0b35fcb69b42d75a3f07c3bbb92 Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Sat, 18 Jan 2025 11:36:26 -0700 Subject: [PATCH] Add Drift correction --- src/main/java/frc4388/robot/Constants.java | 18 +- .../frc4388/robot/subsystems/SwerveDrive.java | 393 +++++++++--------- 2 files changed, 220 insertions(+), 191 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 71734b5..643bdfb 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -84,7 +84,7 @@ public final class Constants { public static final double[] GEARS = {SLOW_SPEED, FAST_SPEED, TURBO_SPEED}; public static final int STARTING_GEAR = 0; - // dimensions + // Dimensions public static final double WIDTH = 18.5; // TODO: validate public static final double HEIGHT = 18.5; // TODO: validate 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_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; + // Operation 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); - private static final class ModuleSpecificConstants { //2025 + /* private static final class ModuleSpecificConstants { //2025 //Front Left private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.229736328125+.25); 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 Distance BACK_RIGHT_XPOS = Inches.of(HALF_WIDTH); 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 private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.36328125); 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 Distance BACK_RIGHT_XPOS = Inches.of(-HALF_WIDTH); private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); - } */ + } public static final class IDs { 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() .withKP(0.1).withKI(0).withKD(0) .withKS(0).withKV(0.124); + + public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1); } public static final class AutoConstants { diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 642c02d..17cf81e 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -43,292 +43,313 @@ public class SwerveDrive extends Subsystem { private SwerveDrivetrain swerveDriveTrain; private Vision vision; - + private int gear_index = SwerveDriveConstants.STARTING_GEAR; private boolean stopped = false; public double speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * SwerveDriveConstants.GEARS[gear_index]; 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 Rotation2d orientRotTarget = new Rotation2d(); public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); /** Creates a new SwerveDrive. */ public SwerveDrive(SwerveDrivetrain swerveDriveTrain, Vision vision) { - // public SwerveDrive(SwerveDrivetrain swerveDriveTrain) { - super(); + // public SwerveDrive(SwerveDrivetrain + // swerveDriveTrain) { + super(); - this.swerveDriveTrain = swerveDriveTrain; - this.vision = vision; + this.swerveDriveTrain = swerveDriveTrain; + this.vision = vision; - RobotConfig config; - try{ - config = RobotConfig.fromGUISettings(); - } catch (Exception e) { - // Handle exception as needed - config = null; - } + RobotConfig config; + try { + config = RobotConfig.fromGUISettings(); + } catch (Exception e) { + // Handle exception as needed + config = null; + } // DoubleSupplier a = () -> 1.d; AutoBuilder.configure( - () -> { + () -> { var pose = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()); // pose = new Pose2d(pose.getX(), pose.getY(), pose.getRotation().times(-1)); - return pose;//getRotation().times(-1) - }, // Robot pose supplier - swerveDriveTrain::resetPose, // Method to reset odometry (will be called if your auto has a starting pose) - () -> swerveDriveTrain.getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - (speeds, feedforwards) -> swerveDriveTrain.setControl(new SwerveRequest.ApplyRobotSpeeds() - .withSpeeds(speeds) - ), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards - new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains - new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants - new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants - ), - config, // The robot configuration - () -> { - // 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 + return pose;// getRotation().times(-1) + }, // Robot pose supplier + swerveDriveTrain::resetPose, // Method to reset odometry (will be called if your auto has a starting + // pose) + () -> swerveDriveTrain.getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + (speeds, feedforwards) -> swerveDriveTrain.setControl(new SwerveRequest.ApplyRobotSpeeds() + .withSpeeds(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. + // Also optionally outputs individual module feedforwards + new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for + // holonomic drive trains + new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants + new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants + ), + config, // The robot configuration + () -> { + // 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(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; - }, - this // Reference to this subsystem to set requirements - ); + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this // Reference to this subsystem to set requirements + ); - } + } - // public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){ - // // double ang = Math.atan2(rightStick.getY(), rightStick.getX()); - // // rightStick.getAngle() - // double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) + Math.pow(leftStick.getY(), 2)); - // // System.out.println(ang); - // // module.go(ang); - // // Rotation2d rot = Rotation2d.fromRadians(ang); - // Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY()); - // SwerveModuleState state = new SwerveModuleState(speed, rot); - // module.setDesiredState(state); + // public void oneModuleTest(SwerveModule module, Translation2d leftStick, + // Translation2d rightStick){ + // // double ang = Math.atan2(rightStick.getY(), rightStick.getX()); + // // rightStick.getAngle() + // double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) + + // Math.pow(leftStick.getY(), 2)); + // // System.out.println(ang); + // // module.go(ang); + // // Rotation2d rot = Rotation2d.fromRadians(ang); + // 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) { - 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 - return; // don't bother doing swerve drive math and return early. + 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 - leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); - - if (fieldRelative) { - swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() - .withVelocityX(leftStick.getX()*speedAdjust) - .withVelocityY(-leftStick.getY()*speedAdjust) - .withRotationalRate(rightStick.getX()*rotSpeedAdjust) - ); - // double rot = 0; - - // ! drift correction - // dependant on if the new odomitry system acounts for rotational drift, this may not be needed. - // 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; - // } + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput + return; // don't bother doing swerve drive math and return early. - // // SmartDashboard.putBoolean("drift correction", true); - - // // rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; + 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); + } - // } + // // 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, 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) - ); - } + // } + + // // 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)); + + // // 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, + // 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 - 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 - return; // don't bother doing swerve drive math and return early. + 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 + 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 - leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); - - swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(leftStick.getX()*speedAdjust) - .withVelocityY(leftStick.getY()*speedAdjust) - .withTargetDirection(rightStick.getAngle()) - ); + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput + return; // don't bother doing swerve drive math and return early. + + leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); + + swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withTargetDirection(rightStick.getAngle())); } public boolean rotateToTarget(double angle) { - swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(0) - .withVelocityY(0) - .withTargetDirection(Rotation2d.fromDegrees(angle)) - ); + swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(0) + .withVelocityY(0) + .withTargetDirection(Rotation2d.fromDegrees(angle))); - if (Math.abs(angle - getGyroAngle()) < 5.0) { - return true; - } + if (Math.abs(angle - getGyroAngle()) < 5.0) { + return true; + } - return false; + return false; } public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) { - // if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: - // stopModules(); // stop the swerve - - // if (leftStick.getNorm() < 0.05) //if no imput - // return; // don't bother doing swerve drive math and return early. + // if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the + // swerve drive is still going: + // stopModules(); // stop the swerve - leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); - - swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(leftStick.getX()*-speedAdjust) - .withVelocityY(leftStick.getY()*speedAdjust) - .withTargetDirection(rot) - ); - // double + // if (leftStick.getNorm() < 0.05) //if no imput + // return; // don't bother doing swerve drive math and return early. + + leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); + + swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX() * -speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withTargetDirection(rot)); + // double } public double getGyroAngle() { - return swerveDriveTrain.getRotation3d().getAngle(); + return swerveDriveTrain.getRotation3d().getAngle(); } public void resetGyro() { - swerveDriveTrain.tareEverything(); + swerveDriveTrain.tareEverything(); } - public void stopModules() { - swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); - } + public void stopModules() { + stopped = true; + swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); + } @Override public void periodic() { - // This method will be called once per scheduler run\ - SmartDashboard.putNumber("Gyro", getGyroAngle()); - SmartDashboard.putNumber("RotTartget", rotTarget); + // This method will be called once per scheduler run\ + SmartDashboard.putNumber("Gyro", getGyroAngle()); + 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()){ - swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time); - } + if (vision.isTag()) { + swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time); + } - // if(e.isPresent()) + // if(e.isPresent()) } 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() { - if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index - int i = gear_index - 1; - if (i == -1) i = 0; - setPercentOutput(SwerveDriveConstants.GEARS[i]); - gear_index = i; + if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) + reset_index(); // If outof bounds: reset index + int i = gear_index - 1; + if (i == -1) + i = 0; + setPercentOutput(SwerveDriveConstants.GEARS[i]); + gear_index = i; } public void shiftUp() { - if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index - int i = gear_index + 1; - if (i == SwerveDriveConstants.GEARS.length) i = SwerveDriveConstants.GEARS.length - 1; - setPercentOutput(SwerveDriveConstants.GEARS[i]); - gear_index = i; + if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) + reset_index(); // If outof bounds: reset index + int i = gear_index + 1; + if (i == SwerveDriveConstants.GEARS.length) + i = SwerveDriveConstants.GEARS.length - 1; + setPercentOutput(SwerveDriveConstants.GEARS[i]); + gear_index = i; } public void setPercentOutput(double speed) { - speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed; - gear_index = -1; + speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed; + gear_index = -1; } public void setToSlow() { - setPercentOutput(SwerveDriveConstants.SLOW_SPEED); - gear_index = 0; + setPercentOutput(SwerveDriveConstants.SLOW_SPEED); + gear_index = 0; } public void setToFast() { - setPercentOutput(SwerveDriveConstants.FAST_SPEED); - gear_index = 1; + setPercentOutput(SwerveDriveConstants.FAST_SPEED); + gear_index = 1; } public void setToTurbo() { - setPercentOutput(SwerveDriveConstants.TURBO_SPEED); - gear_index = 2; + setPercentOutput(SwerveDriveConstants.TURBO_SPEED); + gear_index = 2; } public void shiftUpRot() { - rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED; + rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED; } public void shiftDownRot() { - rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED; + rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED; } @Override public String getSubsystemName() { - return "Swerve Drive Controller"; + return "Swerve Drive Controller"; } ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems") - .getLayout(getSubsystemName(), BuiltInLayouts.kList) - .withSize(2, 2); + .getLayout(getSubsystemName(), BuiltInLayouts.kList) + .withSize(2, 2); GenericEntry sbGyro = subsystemLayout - .add("Gyro angle", 0) - .withWidget(BuiltInWidgets.kGyro) - .getEntry(); + .add("Gyro angle", 0) + .withWidget(BuiltInWidgets.kGyro) + .getEntry(); GenericEntry sbShiftState = subsystemLayout - .add("Shift State", 0) - .withWidget(BuiltInWidgets.kNumberBar) - .getEntry(); + .add("Shift State", 0) + .withWidget(BuiltInWidgets.kNumberBar) + .getEntry(); @Override public void queryStatus() { - sbGyro.setDouble(getGyroAngle()); - sbShiftState.setDouble(this.speedAdjust); - - //TODO: Add more status things + sbGyro.setDouble(getGyroAngle()); + sbShiftState.setDouble(this.speedAdjust); + + // TODO: Add more status things } @Override 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."); - - return 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."); + + return status; } }