From af9782bc98101334f2c1b6d1b9972b01b61eae5a Mon Sep 17 00:00:00 2001 From: Jake Date: Wed, 17 Apr 2024 20:28:18 -0500 Subject: [PATCH 1/7] Allow alliance information to come through when in sim --- src/main/java/frc/robot/util/AllianceUtil.java | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/main/java/frc/robot/util/AllianceUtil.java b/src/main/java/frc/robot/util/AllianceUtil.java index 17587a3..64f26d3 100644 --- a/src/main/java/frc/robot/util/AllianceUtil.java +++ b/src/main/java/frc/robot/util/AllianceUtil.java @@ -2,15 +2,10 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import frc.robot.Robot; public class AllianceUtil { public static boolean isRedAlliance() { - if (Robot.isSimulation()) { - return false; - } - if (DriverStation.getAlliance().isEmpty()) { return false; } From affc8c415032678d5e4c8bd12313d6b5d98fa569 Mon Sep 17 00:00:00 2001 From: Jake Date: Wed, 17 Apr 2024 20:28:36 -0500 Subject: [PATCH 2/7] Set default back to none for auto generator --- .../java/frc/robot/auto/generator/PathPlannerAutoGenerator.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java b/src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java index 466a2be..131b820 100644 --- a/src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java +++ b/src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java @@ -80,7 +80,7 @@ public PathPlannerAutoGenerator(DriveSubsystem drive, ArmSubsystem arm, ShooterS for (String s : autos) { autoSelector.addOption(s, s); } - autoSelector.setDefaultOption("SubC-MidDC", "SubC-MidDC"); + autoSelector.setDefaultOption("None", "None"); autoSelector.onChange((cmd) -> { if(cmd != null){ From da0d6a04a6804b87e0f9355a860e6fd7f91ded4b Mon Sep 17 00:00:00 2001 From: Jake Date: Wed, 17 Apr 2024 20:30:01 -0500 Subject: [PATCH 3/7] Cleanup, refactor and comment drive subsys --- .../java/frc/robot/drive/DriveSubsystem.java | 130 ++++++++++-------- 1 file changed, 73 insertions(+), 57 deletions(-) diff --git a/src/main/java/frc/robot/drive/DriveSubsystem.java b/src/main/java/frc/robot/drive/DriveSubsystem.java index b17e174..48b1bcc 100644 --- a/src/main/java/frc/robot/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/drive/DriveSubsystem.java @@ -7,17 +7,16 @@ import java.io.File; import java.io.IOException; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; -import frc.robot.poseScheduler.PoseScheduler; import swervelib.SwerveDrive; import swervelib.SwerveModule; import swervelib.math.SwerveMath; @@ -27,20 +26,16 @@ public class DriveSubsystem extends SubsystemBase { - private SwerveDrive swerve; + private final SwerveDrive swerve; - private final PoseScheduler poseScheduler; + private final PIDController rotationController = new PIDController(Constants.Drive.ROTATION_P, Constants.Drive.ROTATION_I, Constants.Drive.ROTATION_D); + private Rotation2d rotationTarget = null; - private ShuffleboardTab dashboard; + private ChassisSpeeds inputSpeeds = new ChassisSpeeds(); + private boolean isFieldRelative = true; - private final PIDController rotationController; - private Double targetAngle = null; - private int rotationDirection = 1; //Used to make sure the rotation PID continues working while the gyroscope is inverted. - - public DriveSubsystem(PoseScheduler poseScheduler) { - this.poseScheduler = poseScheduler; - rotationController = new PIDController(Constants.Drive.ROTATION_P, Constants.Drive.ROTATION_I, Constants.Drive.ROTATION_D); + public DriveSubsystem() { rotationController.enableContinuousInput(-180, 180); rotationController.setTolerance(Constants.Drive.ROTATION_TARGET_RANGE); @@ -49,29 +44,28 @@ public DriveSubsystem(PoseScheduler poseScheduler) { swerve = new SwerveParser(new File(Filesystem.getDeployDirectory(), "swerve")).createSwerveDrive( Constants.Drive.MAXIMUM_VELOCITY, SwerveMath.calculateDegreesPerSteeringRotation(Constants.Drive.STEER_GEAR_RATIO, Constants.Drive.PULSE_PER_ROTATION), - SwerveMath.calculateMetersPerRotation( Constants.Drive.WHEEL_DIAMETER_METERS, Constants.Drive.DRIVE_GEAR_RATIO, Constants.Drive.PULSE_PER_ROTATION) + SwerveMath.calculateMetersPerRotation(Constants.Drive.WHEEL_DIAMETER_METERS, Constants.Drive.DRIVE_GEAR_RATIO, Constants.Drive.PULSE_PER_ROTATION) ); } catch (IOException e) { - System.out.println("Swerve drive configuration file could not be found at " + Filesystem.getDeployDirectory() + "/swerve"); e.printStackTrace(); + throw new Error("Swerve drive configuration could not be loaded from " + Filesystem.getDeployDirectory() + "/swerve", e); } - swerve.chassisVelocityCorrection = false; + swerve.chassisVelocityCorrection = false; //TODO test out setting this swerve.setHeadingCorrection(true); initDashboard(); } private void initDashboard() { - dashboard = Shuffleboard.getTab("Drive"); + ShuffleboardTab dashboard = Shuffleboard.getTab("Drive"); dashboard.addString("Current Command", this::getCommandName); dashboard.add("Rotation PID", rotationController); dashboard.addDouble("Yaw", () ->swerve.getYaw().getDegrees()); - dashboard.addDouble("Target angle", () -> targetAngle == null ? -1 : targetAngle); + dashboard.addString("Target angle", () -> rotationTarget == null ? "None" : rotationTarget.toString()); dashboard.addBoolean("Rotation On Target?", () -> rotationController.atSetpoint()); - dashboard.add(swerve.field).withPosition(0, 1).withSize(5, 3); int position = 0; for (SwerveModule m : swerve.getModules()) { @@ -107,37 +101,56 @@ public ChassisSpeeds getRobotRelativeSpeeds() { /** * Used for PathPlanner autonomous + * + * @param override new pose */ - public void setRobotRelativeChassisSpeeds(ChassisSpeeds speeds) { - swerve.drive(new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond), speeds.omegaRadiansPerSecond, false, false); + public void resetOdometry(Pose2d override) { + swerve.resetOdometry(override); } - public void drive(double x, double y, double rotation) { - if (rotation != 0 || (targetAngle != null && rotationController.atSetpoint())) { - targetAngle = null; - } - - if (targetAngle != null) { - rotation = rotationDirection * rotationController.calculate(swerve.getYaw().getDegrees(), targetAngle); - }else { - rotation = rotation * Constants.Drive.MAXIMUM_ANGULAR_VELOCITY; - } - - swerve.drive( - new Translation2d( - x * Constants.Drive.MAXIMUM_VELOCITY, - y * Constants.Drive.MAXIMUM_VELOCITY - ), - rotation, - true, false - ); + /** + * Used for PathPlanner autonomous + * + * @param speeds robot relative chassis speeds + */ + public void setRobotRelativeChassisSpeeds(ChassisSpeeds speeds) { + inputSpeeds = speeds; + isFieldRelative = false; } + /** + * Get the current robot pose as measured by the odometry. + * Odometry may include vision based measurements. + * @return the current pose of the robot + */ public Pose2d getPose() { return swerve.getPose(); } - public void addVisionMeasurement(Pose2d visionMeasuredPose) { + /** + * Drive the robot in field relative mode. + * This is the primary method of teleoperated robot control. + * + * @param x percent translational speed in the x direction + * @param y percent translational speed in the y direction + * @param rotation percent rotation speed + */ + public void drive(double x, double y, double rotation) { + inputSpeeds = new ChassisSpeeds( + x * Constants.Drive.MAXIMUM_VELOCITY, + y * Constants.Drive.MAXIMUM_VELOCITY, + rotation * Constants.Drive.MAXIMUM_ANGULAR_VELOCITY + ); + + isFieldRelative = true; + } + + /** + * Add a vision measurement to the pose estimate. + * @param visionMeasuredPose The pose the vision measured. + * @param measurementTimeSeconds The time the given pose was measured in seconds + */ + public void addVisionMeasurement(Pose2d visionMeasuredPose, double measurementTimeSeconds) { // Per recommendation from lib authors, discard poses which are // too far away from current pose. double distance = Math.sqrt( @@ -146,31 +159,34 @@ public void addVisionMeasurement(Pose2d visionMeasuredPose) { Math.pow((visionMeasuredPose.getY() - swerve.getPose().getY()), 2) ); if (distance <= Constants.Drive.MAXIMUM_VISION_POSE_OVERRIDE_DISTANCE) { - swerve.swerveDrivePoseEstimator.addVisionMeasurement(visionMeasuredPose, Timer.getFPGATimestamp()); + swerve.swerveDrivePoseEstimator.addVisionMeasurement(visionMeasuredPose, measurementTimeSeconds); } } /** - * Used for autonomous - * - * @param override new pose + * Set a rotation target. May be null. + * @param target Rotation target or null to disable target */ - public void resetOdometry(Pose2d override) { - swerve.resetOdometry(override); - } - - public void setTargetAngle(double angleDeg) { - rotationController.reset(); - targetAngle = angleDeg; - } - - public void unsetTargetAngle() { - targetAngle = null; + public void setRotationTarget(Rotation2d target) { + rotationTarget = target; } @Override public void periodic() { - poseScheduler.updatePose(getPose()); + if (inputSpeeds.omegaRadiansPerSecond != 0) { + rotationTarget = null; + }else if (rotationTarget != null) { + inputSpeeds.omegaRadiansPerSecond = rotationController.calculate( + MathUtil.inputModulus(swerve.getYaw().getDegrees(), -180, 180), + MathUtil.inputModulus(rotationTarget.getDegrees(), -180, 180) + ); + } + + if (isFieldRelative) { + swerve.driveFieldOriented(inputSpeeds); + }else { + swerve.drive(inputSpeeds); + } } } From bbaa6867e267c69faa8b4db14efb691398155af6 Mon Sep 17 00:00:00 2001 From: Jake Date: Wed, 17 Apr 2024 20:31:05 -0500 Subject: [PATCH 4/7] Make DriveCommands static class to take logic away from robot container and replace TurnToAngle boilerplate --- .vscode/settings.json | 1 + src/main/java/frc/robot/RobotContainer.java | 16 +++++------- .../java/frc/robot/drive/DriveCommands.java | 22 ++++++++++++++++ .../java/frc/robot/drive/TurnToAngle.java | 25 ------------------- 4 files changed, 29 insertions(+), 35 deletions(-) create mode 100644 src/main/java/frc/robot/drive/DriveCommands.java delete mode 100644 src/main/java/frc/robot/drive/TurnToAngle.java diff --git a/.vscode/settings.json b/.vscode/settings.json index 9196310..0a31e22 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -52,6 +52,7 @@ "sparkmax", "subwoofer", "teleop", + "teleoperated", "traj", "Uper", "wpiblue", diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8417ac2..6dbdb7d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,20 +23,17 @@ import frc.robot.cmdGroup.IntakeNote; import frc.robot.cmdGroup.ShootAndReturnHome; import frc.robot.drive.DefaultDrive; +import frc.robot.drive.DriveCommands; import frc.robot.drive.DriveSubsystem; -import frc.robot.drive.TurnToAngle; import frc.robot.notification.ContextualColor; import frc.robot.notification.LEDSubsystem; import frc.robot.poseScheduler.PoseScheduler; import frc.robot.shooter.ShooterSubsystem; -import frc.robot.shooter.commands.SpinDown; -import frc.robot.shooter.commands.SpinUp; -import frc.robot.shooter.commands.SpinUpAndShoot; import frc.robot.undertaker.UndertakerSubsystem; -import frc.robot.util.RobotContext; -import frc.robot.util.VersionFile; import frc.robot.util.AllianceUtil; import frc.robot.util.Dashboards; +import frc.robot.util.RobotContext; +import frc.robot.util.VersionFile; public class RobotContainer { private final PoseScheduler poseScheduler; @@ -63,7 +60,7 @@ public RobotContainer() { DataLogManager.log("Data log started."); poseScheduler = new PoseScheduler(); - drive = new DriveSubsystem(poseScheduler); + drive = new DriveSubsystem(); //visionSubsystem = VisionSubsystem.create(); ledSubsystem = LEDSubsystem.create(); @@ -96,9 +93,8 @@ private void configureBindings() { () -> robotContext.getShooterSpeed()[1] )); - driverController.a().onTrue(new TurnToAngle(drive, AllianceUtil.isRedAlliance() ? 0 : 180)); - driverController.b().onTrue(new TurnToAngle(drive, AllianceUtil.isRedAlliance() ? 150 : -30.5)); //TODO alliance switching - driverController.x().onTrue(new TurnToAngle(drive, AllianceUtil.isRedAlliance() ? 90 : -90)); + driverController.a().onTrue(DriveCommands.turnToShootCommand(drive)); + driverController.x().onTrue(DriveCommands.turnToAmpCommand(drive)); operatorController.leftTrigger().onTrue(new MoveToPosition(arm, Constants.Arm.AMP_POSITION)); operatorController.rightTrigger().onTrue(new MoveToHome(arm)); diff --git a/src/main/java/frc/robot/drive/DriveCommands.java b/src/main/java/frc/robot/drive/DriveCommands.java new file mode 100644 index 0000000..0c07b7e --- /dev/null +++ b/src/main/java/frc/robot/drive/DriveCommands.java @@ -0,0 +1,22 @@ +package frc.robot.drive; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.util.AllianceUtil; + +public class DriveCommands { + + public static Command turnToShootCommand(DriveSubsystem drive) { + return new InstantCommand(() -> drive.setRotationTarget(Rotation2d.fromDegrees(AllianceUtil.isRedAlliance() ? 0 : 180)), drive); + } + + public static Command turnToAmpCommand(DriveSubsystem drive) { + return new InstantCommand(() -> drive.setRotationTarget(Rotation2d.fromDegrees(-90)), drive); + } + + public static Command zeroGyroscope(DriveSubsystem drive) { + return new InstantCommand(drive::zeroGyroscope, drive); + } + +} diff --git a/src/main/java/frc/robot/drive/TurnToAngle.java b/src/main/java/frc/robot/drive/TurnToAngle.java deleted file mode 100644 index a27d4b6..0000000 --- a/src/main/java/frc/robot/drive/TurnToAngle.java +++ /dev/null @@ -1,25 +0,0 @@ -package frc.robot.drive; - -import edu.wpi.first.wpilibj2.command.Command; - -public class TurnToAngle extends Command { - - private final DriveSubsystem drive; - private final double targetAngle; - - public TurnToAngle(DriveSubsystem drive, double angleDeg) { - this.drive = drive; - this.targetAngle = angleDeg; - addRequirements(drive); - } - - @Override - public void initialize() { - drive.setTargetAngle(targetAngle); - } - - @Override - public boolean isFinished() { - return true; - } -} From 23a238e1733019c177959c65d79e4dba6ed9a682 Mon Sep 17 00:00:00 2001 From: Jake Date: Thu, 18 Apr 2024 01:49:28 -0500 Subject: [PATCH 5/7] Delete vision command which was preventing compilation See other PR for new vision integration strategy --- .../java/frc/robot/vision/UpdatePose.java | 28 ------------------- 1 file changed, 28 deletions(-) delete mode 100644 src/main/java/frc/robot/vision/UpdatePose.java diff --git a/src/main/java/frc/robot/vision/UpdatePose.java b/src/main/java/frc/robot/vision/UpdatePose.java deleted file mode 100644 index 5e6b8be..0000000 --- a/src/main/java/frc/robot/vision/UpdatePose.java +++ /dev/null @@ -1,28 +0,0 @@ -package frc.robot.vision; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.drive.DriveSubsystem; - -public class UpdatePose extends Command{ - - private final VisionSubsystem vision; - private final DriveSubsystem drive; - - public UpdatePose(VisionSubsystem vision, DriveSubsystem drive){ - addRequirements(vision); - this.vision = vision; - this.drive = drive; - } - - @Override - public void execute(){ - if(vision.hasTarget()){ - drive.addVisionMeasurement(vision.getBotpose()); - } - } - - @Override - public boolean isFinished(){ - return false; - } -} From b1951984c79aa7efdbed4febd3b51ac005062519 Mon Sep 17 00:00:00 2001 From: Jake Herrmann Date: Thu, 18 Apr 2024 21:29:38 -0500 Subject: [PATCH 6/7] Change rotation target logging --- src/main/java/frc/robot/drive/DriveSubsystem.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/drive/DriveSubsystem.java b/src/main/java/frc/robot/drive/DriveSubsystem.java index 48b1bcc..808d2ad 100644 --- a/src/main/java/frc/robot/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/drive/DriveSubsystem.java @@ -62,7 +62,8 @@ private void initDashboard() { dashboard.add("Rotation PID", rotationController); dashboard.addDouble("Yaw", () ->swerve.getYaw().getDegrees()); - dashboard.addString("Target angle", () -> rotationTarget == null ? "None" : rotationTarget.toString()); + dashboard.addDouble("Target angle", () -> rotationTarget == null ? -1 : rotationTarget.getDegrees()); + dashboard.addBoolean("Has rotation target?", () -> rotationTarget == null); dashboard.addBoolean("Rotation On Target?", () -> rotationController.atSetpoint()); From c3dd7d9b73be3029f1600e612f9810228006ec18 Mon Sep 17 00:00:00 2001 From: Josh Pordon Date: Sat, 20 Apr 2024 17:26:11 -0500 Subject: [PATCH 7/7] merge with main --- src/main/java/frc/robot/RobotContainer.java | 8 +- .../java/frc/robot/drive/DriveCommands.java | 6 +- .../java/frc/robot/drive/DriveSubsystem.java | 231 +++++++++--------- 3 files changed, 123 insertions(+), 122 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 269ba8b..92cd968 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -59,8 +59,8 @@ public RobotContainer() { DriverStation.startDataLog(DataLogManager.getLog()); DataLogManager.log("Data log started."); - poseScheduler = new PoseScheduler(); - drive = new DriveSubsystem(); + poseScheduler = new PoseScheduler(); + drive = new DriveSubsystem(); // visionSubsystem = VisionSubsystem.create(); ledSubsystem = LEDSubsystem.create(); @@ -93,8 +93,8 @@ private void configureBindings() { .onTrue(new ShootAndReturnHome(shooter, arm, () -> robotContext.getShooterSpeed()[0], () -> robotContext .getShooterSpeed()[1])); - driverController.a().onTrue(DriveCommands.turnToShootCommand(drive)); - driverController.x().onTrue(DriveCommands.turnToAmpCommand(drive)); + driverController.a().onTrue(DriveCommands.turnToShootCommand(drive)); + driverController.x().onTrue(DriveCommands.turnToAmpCommand(drive)); operatorController.leftTrigger().onTrue(new MoveToPosition(arm, Constants.Arm.AMP_POSITION)); operatorController.rightTrigger().onTrue(new MoveToHome(arm)); diff --git a/src/main/java/frc/robot/drive/DriveCommands.java b/src/main/java/frc/robot/drive/DriveCommands.java index 0c07b7e..4e80e92 100644 --- a/src/main/java/frc/robot/drive/DriveCommands.java +++ b/src/main/java/frc/robot/drive/DriveCommands.java @@ -6,9 +6,10 @@ import frc.robot.util.AllianceUtil; public class DriveCommands { - + public static Command turnToShootCommand(DriveSubsystem drive) { - return new InstantCommand(() -> drive.setRotationTarget(Rotation2d.fromDegrees(AllianceUtil.isRedAlliance() ? 0 : 180)), drive); + return new InstantCommand( + () -> drive.setRotationTarget(Rotation2d.fromDegrees(AllianceUtil.isRedAlliance() ? 0 : 180)), drive); } public static Command turnToAmpCommand(DriveSubsystem drive) { @@ -18,5 +19,4 @@ public static Command turnToAmpCommand(DriveSubsystem drive) { public static Command zeroGyroscope(DriveSubsystem drive) { return new InstantCommand(drive::zeroGyroscope, drive); } - } diff --git a/src/main/java/frc/robot/drive/DriveSubsystem.java b/src/main/java/frc/robot/drive/DriveSubsystem.java index fde2975..283536d 100644 --- a/src/main/java/frc/robot/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/drive/DriveSubsystem.java @@ -14,7 +14,6 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; -import frc.robot.poseScheduler.PoseScheduler; import java.io.File; import java.io.IOException; import swervelib.SwerveDrive; @@ -26,36 +25,43 @@ public class DriveSubsystem extends SubsystemBase { - private final SwerveDrive swerve; - - private final PIDController rotationController = new PIDController(Constants.Drive.ROTATION_P, Constants.Drive.ROTATION_I, Constants.Drive.ROTATION_D); - private Rotation2d rotationTarget = null; - - private ChassisSpeeds inputSpeeds = new ChassisSpeeds(); - private boolean isFieldRelative = true; - - - public DriveSubsystem() { - rotationController.enableContinuousInput(-180, 180); - rotationController.setTolerance(Constants.Drive.ROTATION_TARGET_RANGE); + private final SwerveDrive swerve; + + private final PIDController rotationController = + new PIDController(Constants.Drive.ROTATION_P, Constants.Drive.ROTATION_I, Constants.Drive.ROTATION_D); + private Rotation2d rotationTarget = null; + + private ChassisSpeeds inputSpeeds = new ChassisSpeeds(); + private boolean isFieldRelative = true; + + public DriveSubsystem() { + rotationController.enableContinuousInput(-180, 180); + rotationController.setTolerance(Constants.Drive.ROTATION_TARGET_RANGE); + + SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; + try { + swerve = new SwerveParser(new File(Filesystem.getDeployDirectory(), "swerve")) + .createSwerveDrive( + Constants.Drive.MAXIMUM_VELOCITY, + SwerveMath.calculateDegreesPerSteeringRotation( + Constants.Drive.STEER_GEAR_RATIO, Constants.Drive.PULSE_PER_ROTATION), + SwerveMath.calculateMetersPerRotation( + Constants.Drive.WHEEL_DIAMETER_METERS, + Constants.Drive.DRIVE_GEAR_RATIO, + Constants.Drive.PULSE_PER_ROTATION)); + } catch (IOException e) { + e.printStackTrace(); + throw new Error( + "Swerve drive configuration could not be loaded from " + Filesystem.getDeployDirectory() + + "/swerve", + e); + } - SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; - try { - swerve = new SwerveParser(new File(Filesystem.getDeployDirectory(), "swerve")).createSwerveDrive( - Constants.Drive.MAXIMUM_VELOCITY, - SwerveMath.calculateDegreesPerSteeringRotation(Constants.Drive.STEER_GEAR_RATIO, Constants.Drive.PULSE_PER_ROTATION), - SwerveMath.calculateMetersPerRotation(Constants.Drive.WHEEL_DIAMETER_METERS, Constants.Drive.DRIVE_GEAR_RATIO, Constants.Drive.PULSE_PER_ROTATION) - ); - } catch (IOException e) { - e.printStackTrace(); - throw new Error("Swerve drive configuration could not be loaded from " + Filesystem.getDeployDirectory() + "/swerve", e); + swerve.chassisVelocityCorrection = false; // TODO test out setting this + swerve.setHeadingCorrection(true); + initDashboard(); } - swerve.chassisVelocityCorrection = false; //TODO test out setting this - swerve.setHeadingCorrection(true); - initDashboard(); - } - private void initDashboard() { ShuffleboardTab dashboard = Shuffleboard.getTab("Drive"); dashboard.addString("Current Command", this::getCommandName); @@ -63,9 +69,9 @@ private void initDashboard() { dashboard.add("Rotation PID", rotationController); dashboard.addDouble("Yaw", () -> swerve.getYaw().getDegrees()); dashboard.addDouble("Target angle", () -> rotationTarget == null ? -1 : rotationTarget.getDegrees()); - dashboard.addBoolean("Has rotation target?", () -> rotationTarget == null); + dashboard.addBoolean("Has rotation target?", () -> rotationTarget == null); - dashboard.addBoolean("Rotation On Target?", () -> rotationController.atSetpoint()); + dashboard.addBoolean("Rotation On Target?", () -> rotationController.atSetpoint()); dashboard.add(swerve.field).withPosition(0, 1).withSize(5, 3); int position = 0; @@ -102,94 +108,89 @@ public ChassisSpeeds getRobotRelativeSpeeds() { return swerve.getRobotVelocity(); } - /** - * Used for PathPlanner autonomous - * - * @param override new pose - */ - public void resetOdometry(Pose2d override) { - swerve.resetOdometry(override); - } - - /** - * Used for PathPlanner autonomous - * - * @param speeds robot relative chassis speeds - */ - public void setRobotRelativeChassisSpeeds(ChassisSpeeds speeds) { - inputSpeeds = speeds; - isFieldRelative = false; - } - - /** - * Get the current robot pose as measured by the odometry. - * Odometry may include vision based measurements. - * @return the current pose of the robot - */ - public Pose2d getPose() { - return swerve.getPose(); - } - - /** - * Drive the robot in field relative mode. - * This is the primary method of teleoperated robot control. - * - * @param x percent translational speed in the x direction - * @param y percent translational speed in the y direction - * @param rotation percent rotation speed - */ - public void drive(double x, double y, double rotation) { - inputSpeeds = new ChassisSpeeds( - x * Constants.Drive.MAXIMUM_VELOCITY, - y * Constants.Drive.MAXIMUM_VELOCITY, - rotation * Constants.Drive.MAXIMUM_ANGULAR_VELOCITY - ); - - isFieldRelative = true; - } - - /** - * Add a vision measurement to the pose estimate. - * @param visionMeasuredPose The pose the vision measured. - * @param measurementTimeSeconds The time the given pose was measured in seconds - */ - public void addVisionMeasurement(Pose2d visionMeasuredPose, double measurementTimeSeconds) { - // Per recommendation from lib authors, discard poses which are - // too far away from current pose. - double distance = Math.sqrt( - Math.pow((visionMeasuredPose.getX() - swerve.getPose().getX()), 2) - + - Math.pow((visionMeasuredPose.getY() - swerve.getPose().getY()), 2) - ); - if (distance <= Constants.Drive.MAXIMUM_VISION_POSE_OVERRIDE_DISTANCE) { - swerve.swerveDrivePoseEstimator.addVisionMeasurement(visionMeasuredPose, measurementTimeSeconds); + /** + * Used for PathPlanner autonomous + * + * @param override new pose + */ + public void resetOdometry(Pose2d override) { + swerve.resetOdometry(override); + } + + /** + * Used for PathPlanner autonomous + * + * @param speeds robot relative chassis speeds + */ + public void setRobotRelativeChassisSpeeds(ChassisSpeeds speeds) { + inputSpeeds = speeds; + isFieldRelative = false; + } + + /** + * Get the current robot pose as measured by the odometry. + * Odometry may include vision based measurements. + * @return the current pose of the robot + */ + public Pose2d getPose() { + return swerve.getPose(); + } + + /** + * Drive the robot in field relative mode. + * This is the primary method of teleoperated robot control. + * + * @param x percent translational speed in the x direction + * @param y percent translational speed in the y direction + * @param rotation percent rotation speed + */ + public void drive(double x, double y, double rotation) { + inputSpeeds = new ChassisSpeeds( + x * Constants.Drive.MAXIMUM_VELOCITY, + y * Constants.Drive.MAXIMUM_VELOCITY, + rotation * Constants.Drive.MAXIMUM_ANGULAR_VELOCITY); + + isFieldRelative = true; + } + + /** + * Add a vision measurement to the pose estimate. + * @param visionMeasuredPose The pose the vision measured. + * @param measurementTimeSeconds The time the given pose was measured in seconds + */ + public void addVisionMeasurement(Pose2d visionMeasuredPose, double measurementTimeSeconds) { + // Per recommendation from lib authors, discard poses which are + // too far away from current pose. + double distance = + Math.sqrt(Math.pow((visionMeasuredPose.getX() - swerve.getPose().getX()), 2) + + Math.pow((visionMeasuredPose.getY() - swerve.getPose().getY()), 2)); + if (distance <= Constants.Drive.MAXIMUM_VISION_POSE_OVERRIDE_DISTANCE) { + swerve.swerveDrivePoseEstimator.addVisionMeasurement(visionMeasuredPose, measurementTimeSeconds); + } } - } - - /** - * Set a rotation target. May be null. - * @param target Rotation target or null to disable target - */ - public void setRotationTarget(Rotation2d target) { - rotationTarget = target; - } - - @Override - public void periodic() { - if (inputSpeeds.omegaRadiansPerSecond != 0) { - rotationTarget = null; - }else if (rotationTarget != null) { - inputSpeeds.omegaRadiansPerSecond = rotationController.calculate( - MathUtil.inputModulus(swerve.getYaw().getDegrees(), -180, 180), - MathUtil.inputModulus(rotationTarget.getDegrees(), -180, 180) - ); + /** + * Set a rotation target. May be null. + * @param target Rotation target or null to disable target + */ + public void setRotationTarget(Rotation2d target) { + rotationTarget = target; } - if (isFieldRelative) { - swerve.driveFieldOriented(inputSpeeds); - }else { - swerve.drive(inputSpeeds); + @Override + public void periodic() { + if (inputSpeeds.omegaRadiansPerSecond != 0) { + rotationTarget = null; + } else if (rotationTarget != null) { + inputSpeeds.omegaRadiansPerSecond = rotationController.calculate( + MathUtil.inputModulus(swerve.getYaw().getDegrees(), -180, 180), + MathUtil.inputModulus(rotationTarget.getDegrees(), -180, 180)); + } + + if (isFieldRelative) { + swerve.driveFieldOriented(inputSpeeds); + } else { + swerve.drive(inputSpeeds); + } } - } }