From b06e9cc09ff825c14aef4f90b7c8a4cc93117883 Mon Sep 17 00:00:00 2001 From: Evan Overman Date: Tue, 10 Oct 2023 15:29:14 -0700 Subject: [PATCH 1/8] broader adoption of `Angle` class --- src/main/java/frc/robot/subsystems/Angle.java | 15 +++++ .../java/frc/robot/systems/AutoBalance.java | 11 ++-- src/main/java/frc/robot/systems/Pigeon.java | 58 ++++++++++--------- 3 files changed, 52 insertions(+), 32 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Angle.java b/src/main/java/frc/robot/subsystems/Angle.java index 292f96f..9e25539 100644 --- a/src/main/java/frc/robot/subsystems/Angle.java +++ b/src/main/java/frc/robot/subsystems/Angle.java @@ -39,6 +39,16 @@ public Angle sub(Angle other) { return angle; } + public Angle mul(double scalar) { + var angle = new Angle(); + angle.dec = this.dec * scalar; + return angle; + } + + public Angle div(double scalar) { + return this.mul(1 / scalar); + } + /** * Creates a new identical angle so that modification of the original will not * effect the value returned from this function. @@ -48,4 +58,9 @@ public Angle clone() { ang.dec = this.dec; return ang; } + + @Override + public String toString() { + return this.degrees() + "°"; + } } diff --git a/src/main/java/frc/robot/systems/AutoBalance.java b/src/main/java/frc/robot/systems/AutoBalance.java index 4b3c4c0..5578134 100644 --- a/src/main/java/frc/robot/systems/AutoBalance.java +++ b/src/main/java/frc/robot/systems/AutoBalance.java @@ -159,14 +159,15 @@ private Stage pause() { * Returns true if auto balance beleives it is balanced, not 100% accurate. */ private boolean level() { - return Math.abs(Pigeon.getPitch().degrees()) < 10.0 && Math.abs(Pigeon.getChangePerSecond().pitchPerSecond) < 10.0; + return Math.abs(Pigeon.getPitch().degrees()) < 10.0 + && Math.abs(Pigeon.getChangePerSecond().pitchPerSecond.degrees()) < 10.0; } /** * Returns true if auto balance beleives it is spiking in angle. */ private boolean spiking() { - return Math.abs(Pigeon.getChangePerSecond().pitchPerSecond) > 10.0; + return Math.abs(Pigeon.getChangePerSecond().pitchPerSecond.degrees()) > 10.0; } /** @@ -174,8 +175,8 @@ private boolean spiking() { */ @Override public void print() { - SmartDashboard.putString("Auto Balance Stage: ", instance.stage.toString()); - SmartDashboard.putBoolean("Auto Balance Spiking ", spiking()); - SmartDashboard.putBoolean("Auto Balance Balanced ", level()); + SmartDashboard.putString("Auto Balance Stage", instance.stage.toString()); + SmartDashboard.putBoolean("Auto Balance Spiking", spiking()); + SmartDashboard.putBoolean("Auto Balance Balanced", level()); } } diff --git a/src/main/java/frc/robot/systems/Pigeon.java b/src/main/java/frc/robot/systems/Pigeon.java index 2b09080..ece930e 100644 --- a/src/main/java/frc/robot/systems/Pigeon.java +++ b/src/main/java/frc/robot/systems/Pigeon.java @@ -84,15 +84,15 @@ public static Angle getRoll() { @Override public void print() { - SmartDashboard.putNumber("Pigeon Yaw", getYaw().radians()); - SmartDashboard.putNumber("Pigeon Pitch", getPitch().radians()); - SmartDashboard.putNumber("Pigeon Roll", getRoll().radians()); + SmartDashboard.putString("Pigeon Yaw", getYaw().toString()); + SmartDashboard.putString("Pigeon Pitch", getPitch().toString()); + SmartDashboard.putString("Pigeon Roll", getRoll().toString()); OrientationalChange change = getChangePerSecond(); - SmartDashboard.putNumber("Pigeon Yaw/Sec", change.yawPerSecond); - SmartDashboard.putNumber("Pigeon Pitch/Sec", change.pitchPerSecond); - SmartDashboard.putNumber("Pigeon Roll/Sec", change.rollPerSecond); + SmartDashboard.putString("Pigeon Yaw/Sec", change.yawPerSecond.toString()); + SmartDashboard.putString("Pigeon Pitch/Sec", change.pitchPerSecond.toString()); + SmartDashboard.putString("Pigeon Roll/Sec", change.rollPerSecond.toString()); } /** @@ -100,14 +100,18 @@ public void print() { * calculated from the Pigeon. */ public static class OrientationalChange { - public final double yawPerSecond; - public final double rollPerSecond; - public final double pitchPerSecond; - - private OrientationalChange(double yaw, double roll, double pitch) { - this.yawPerSecond = yaw; - this.rollPerSecond = roll; - this.pitchPerSecond = pitch; + public final Angle yawPerSecond; + public final Angle rollPerSecond; + public final Angle pitchPerSecond; + + /** + * Clones all given angles allowing the caller to mutate the passed in + * references freely, withoutr modifying this class' state. + */ + private OrientationalChange(Angle yaw, Angle roll, Angle pitch) { + this.yawPerSecond = yaw.clone(); + this.rollPerSecond = roll.clone(); + this.pitchPerSecond = pitch.clone(); } } @@ -121,35 +125,35 @@ private class OrientationalChangeCalculator implements Runnable { private Instant recordedInstant; - private double previousYaw; - private double previousRoll; - private double previousPitch; + private Angle previousYaw; + private Angle previousRoll; + private Angle previousPitch; private OrientationalChangeCalculator(Pigeon pigeon) { this.pigeon = pigeon; clock = Clock.systemDefaultZone(); recordedInstant = clock.instant(); - previousYaw = pigeon.pigeon.getYaw(); - previousRoll = pigeon.pigeon.getPitch(); // Roll and Pitch are swapped cause of the way its mounted. - previousPitch = pigeon.pigeon.getRoll(); + previousYaw = new Angle().setDegrees(pigeon.pigeon.getYaw()); + previousRoll = new Angle().setDegrees(pigeon.pigeon.getPitch()); // Roll and Pitch are swapped cause of the way its mounted. + previousPitch = new Angle().setDegrees(pigeon.pigeon.getRoll()); } @Override public void run() { Instant previousInstant = recordedInstant; - double yaw = pigeon.pigeon.getYaw(); - double roll = pigeon.pigeon.getPitch(); // Roll and Pitch are swapped cause of the way its mounted. - double pitch = -pigeon.pigeon.getRoll(); // Negative since it should be positive going up. - + Angle yaw = new Angle().setDegrees(pigeon.pigeon.getYaw()); + Angle roll = new Angle().setDegrees(pigeon.pigeon.getPitch()); // Roll and Pitch are swapped cause of the way its mounted. + Angle pitch = new Angle().setDegrees(pigeon.pigeon.getRoll()); + recordedInstant = clock.instant(); double differenceSeconds = (double)(recordedInstant.toEpochMilli() - previousInstant.toEpochMilli()) / 1000.0; - double changeYaw = (yaw - previousYaw) / differenceSeconds; - double changeRoll = (roll - previousRoll) / differenceSeconds; - double changePitch = -((pitch - previousPitch) / differenceSeconds); + Angle changeYaw = yaw.sub(previousYaw).div(differenceSeconds); + Angle changeRoll = roll.sub(previousRoll).div(differenceSeconds); + Angle changePitch = pitch.sub(previousPitch).div(differenceSeconds); pigeon.changePerSecond = new OrientationalChange(changeYaw, changeRoll, changePitch); From 7e5d89e2751659b76964bc56edec63c1f13b4063 Mon Sep 17 00:00:00 2001 From: Evan Overman Date: Tue, 10 Oct 2023 16:16:10 -0700 Subject: [PATCH 2/8] rename members of `SwerveDrive` --- .../java/frc/robot/systems/SwerveDrive.java | 110 +++++++++--------- 1 file changed, 57 insertions(+), 53 deletions(-) diff --git a/src/main/java/frc/robot/systems/SwerveDrive.java b/src/main/java/frc/robot/systems/SwerveDrive.java index cd94f5d..b89a40c 100644 --- a/src/main/java/frc/robot/systems/SwerveDrive.java +++ b/src/main/java/frc/robot/systems/SwerveDrive.java @@ -62,10 +62,10 @@ public class SwerveDrive extends SmartPrintable { private final SwerveDriveOdometry odometry; // Fully mutable state objects - private BiFunction directionCurve = Controls::defaultCurveTwoDimensional; - private BiFunction inactiveDirectionCurve = null; - private Function turnCurve = Controls::defaultCurve; - private Function inactiveTurnCurve = null; + private BiFunction translationCurve = Controls::defaultCurveTwoDimensional; + private BiFunction inactiveTransationCurve = null; + private Function rotationCurve = Controls::defaultCurve; + private Function inactiveRotationCurve = null; private SwerveMode mode = SwerveMode.HEADLESS; private SwerveMode inactiveMode = null; private SwerveMode displayMode = SwerveMode.HEADLESS; @@ -195,15 +195,15 @@ public static double getRotationSpeed() { * argument is what should be curved, the second is used for context. Return * the curved direction. */ - public static void setDirectionalCurve(BiFunction curve) { - instance.directionCurve = curve; + public static void setTranslationCurve(BiFunction curve) { + instance.translationCurve = curve; } /** * Gets the current curve used for directional inputs. */ - public static BiFunction getDirectionalCurve() { - return instance.directionCurve; + public static BiFunction getTranslationCurve() { + return instance.translationCurve; } /** @@ -213,20 +213,20 @@ public static BiFunction getDirectionalCurve() { * argument is what should be curved, the second is used for context. Return * the curved direction. */ - public static void tempDirectionalCurve(BiFunction curve) { - if (instance.inactiveDirectionCurve != null) { - instance.directionCurve = curve; + public static void tempTranslationCurve(BiFunction curve) { + if (instance.inactiveTransationCurve != null) { + instance.translationCurve = curve; return; } - instance.inactiveDirectionCurve = instance.directionCurve; - instance.directionCurve = curve; + instance.inactiveTransationCurve = instance.translationCurve; + instance.translationCurve = curve; } /** * Exactly like `tempDirectionalCurve` but predicated on a boolean condition. */ - public static void conditionalTempDirectionalCurve( + public static void conditionalTempTranslationCurve( BiFunction curve, boolean condition ) { @@ -234,42 +234,42 @@ public static void conditionalTempDirectionalCurve( return; } - tempDirectionalCurve(curve); + tempTranslationCurve(curve); } /** * Sets the curve function for turn inputs. * @param curve The Function to use for proccessing the curve. */ - public static void setTurnCurve(Function curve) { - instance.turnCurve = curve; + public static void setRotationCurve(Function curve) { + instance.rotationCurve = curve; } /** * Gets the Function currently used for turning. */ - public static Function getTurnCurve() { - return instance.turnCurve; + public static Function getRotationCurve() { + return instance.rotationCurve; } /** * Temporarily sets the curve function for turn inputs. Undone after running. * @param curve The Function to use for proccessing the curve. */ - public static void tempTurnCurve(Function curve) { - if (instance.inactiveTurnCurve != null) { - instance.turnCurve = curve; + public static void tempRotationCurve(Function curve) { + if (instance.inactiveRotationCurve != null) { + instance.rotationCurve = curve; return; } - instance.inactiveTurnCurve = instance.turnCurve; - instance.turnCurve = curve; + instance.inactiveRotationCurve = instance.rotationCurve; + instance.rotationCurve = curve; } /** * Exactly like `tempTurnCurve` but predicated on a boolean condition. */ - public static void conditionalTempTurnCurve( + public static void conditionalTempRotationCurve( Function curve, boolean condition ) { @@ -277,7 +277,7 @@ public static void conditionalTempTurnCurve( return; } - tempTurnCurve(curve); + tempRotationCurve(curve); } /** @@ -312,18 +312,12 @@ public static void run( double rotation ) { speed = Math.abs(translationX) <= 0.05 && Math.abs(translationY) <= 0.05 ? 0.0 : speed; - - SmartDashboard.putNumber("direction x - run 0", translationX); - SmartDashboard.putNumber("direction y - run 0", translationY); // angle is in radians as per Java's trig methods. var angle = Math.atan2(translationY, translationX); translationX = Math.cos(angle) * speed; - translationY = Math.sin(angle) * speed; - - SmartDashboard.putNumber("direction x - run 1", translationX); - SmartDashboard.putNumber("direction y - run 1", translationY); + translationY = Math.sin(angle) * speed; run(translationX, translationY, rotation); } @@ -336,11 +330,9 @@ public static void run( * @param lowSense The angle to move in low sensitivity in degrees, -1 for no movement. */ public static void run(double translationX, double translationY, double rotation) { - var x = instance.directionCurve.apply(translationX, translationY); - var y = instance.directionCurve.apply(translationY, translationX); - SmartDashboard.putNumber("direction x - run 2", x); - SmartDashboard.putNumber("direction y - run 2", y); - rotation = instance.turnCurve.apply(rotation); + var x = instance.translationCurve.apply(translationX, translationY); + var y = instance.translationCurve.apply(translationY, translationX); + rotation = instance.rotationCurve.apply(rotation); runUncurved(x, y, rotation); } @@ -353,6 +345,10 @@ public static void run(double translationX, double translationY, double rotation * @param rotation Rate of turn. -1.0 - 1.0 */ public static void runUncurved(double translationX, double translationY, double rotation) { + instance.translationSpeedX = translationX; + instance.translationSpeedY = translationY; + instance.rotationSpeed = rotation; + SwerveModuleState[] moduleStates = new SwerveModuleState[instance.modules.length]; boolean holdPos = false; @@ -360,7 +356,9 @@ public static void runUncurved(double translationX, double translationY, double case HEADLESS: moduleStates = instance.kinematics.toSwerveModuleStates( ChassisSpeeds.fromFieldRelativeSpeeds( - translationX, -translationY, rotation, + instance.translationSpeedX, + -instance.translationSpeedY, + instance.rotationSpeed, new Rotation2d(Pigeon.getYaw().radians()) ) ); @@ -368,7 +366,11 @@ public static void runUncurved(double translationX, double translationY, double case RELATIVE: moduleStates = instance.kinematics.toSwerveModuleStates( - new ChassisSpeeds(translationX, -translationY, rotation) + new ChassisSpeeds( + instance.translationSpeedX, + -instance.translationSpeedY, + instance.rotationSpeed + ) ); break; @@ -377,7 +379,12 @@ public static void runUncurved(double translationX, double translationY, double holdPos = true; for (int i = 0; i < instance.modules.length; i++) { - moduleStates[i] = new SwerveModuleState(0.0, new Rotation2d(MODULE_ROCK_MODE_POSITIONS[i].radians())); + moduleStates[i] = new SwerveModuleState( + 0.0, + new Rotation2d( + MODULE_ROCK_MODE_POSITIONS[i].radians() + ) + ); } break; @@ -394,12 +401,9 @@ public static void runUncurved(double translationX, double translationY, double } instance.odometry.update(new Rotation2d(Pigeon.getYaw().radians()), instance.positions); - - // Reset temp state - - instance.translationSpeedX = translationX; - instance.translationSpeedY = translationY; - instance.rotationSpeed = rotation; + + // Reset temporary states + instance.displayMode = instance.mode; if (instance.inactiveMode != null) { @@ -407,14 +411,14 @@ public static void runUncurved(double translationX, double translationY, double instance.inactiveMode = null; } - if (instance.inactiveDirectionCurve != null) { - instance.directionCurve = instance.inactiveDirectionCurve; - instance.inactiveDirectionCurve = null; + if (instance.inactiveTransationCurve != null) { + instance.translationCurve = instance.inactiveTransationCurve; + instance.inactiveTransationCurve = null; } - if (instance.inactiveTurnCurve != null) { - instance.turnCurve = instance.inactiveTurnCurve; - instance.inactiveTurnCurve = null; + if (instance.inactiveRotationCurve != null) { + instance.rotationCurve = instance.inactiveRotationCurve; + instance.inactiveRotationCurve = null; } } From ba27ae592474ea6ca90e16eeda400774f86a18a1 Mon Sep 17 00:00:00 2001 From: Evan Overman Date: Tue, 10 Oct 2023 16:16:54 -0700 Subject: [PATCH 3/8] remove unused put --- src/main/java/frc/robot/subsystems/SwerveModule.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index b550456..033bdd2 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -195,7 +195,6 @@ public void setDesiredState(SwerveModuleState state) { } double calculation = rotationController.calculate(currentPosition, (state.angle.getRadians() + Angle.TAU) % Angle.TAU); - SmartDashboard.putNumber("Module " + physicalPosition.asString() + " Rotation Calculation", calculation); rotationMotor.set(calculation); position.angle = new Rotation2d(angularEncoder.getPosition()); From 14945dae771b88f89c806166805f0de6ea5832bc Mon Sep 17 00:00:00 2001 From: Evan Overman Date: Tue, 10 Oct 2023 17:04:35 -0700 Subject: [PATCH 4/8] renames in `Robot` --- src/main/java/frc/robot/Robot.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5158854..dfab6cf 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -72,8 +72,8 @@ public static ControlMode getControlMode() { @Override public void robotInit() { controlMode = ControlMode.DISABLED; - SwerveDrive.setTurnCurve(Controls::defaultCurve); - SwerveDrive.setDirectionalCurve(Controls::defaultCurveTwoDimensional); + SwerveDrive.setRotationCurve(Controls::defaultCurve); + SwerveDrive.setTranslationCurve(Controls::defaultCurveTwoDimensional); } /** @@ -167,15 +167,15 @@ public void teleopPeriodic() { ); } - SwerveDrive.conditionalTempDirectionalCurve( + SwerveDrive.conditionalTempTranslationCurve( Controls.cardinalLock(Controls::defaultCurveTwoDimensional), controllerOne.getXButton() ); // Lock to cardinal directions. - SwerveDrive.conditionalTempDirectionalCurve( + SwerveDrive.conditionalTempTranslationCurve( (x, y) -> Controls.defaultCurveTwoDimensional(x, y) / 3.0, controllerOne.getRightBumper() ); // Half translation speed. - SwerveDrive.conditionalTempDirectionalCurve( + SwerveDrive.conditionalTempTranslationCurve( Controls.cardinalLock((x, y) -> Controls.defaultCurveTwoDimensional(x, y) / 2.0), controllerOne.getRightBumper() && controllerOne.getXButton() ); // Half translation speed. From b8c30ae24aa9593a47721216f47e6ed09ba36051 Mon Sep 17 00:00:00 2001 From: Evan Overman Date: Tue, 10 Oct 2023 17:04:45 -0700 Subject: [PATCH 5/8] modules threading --- .../frc/robot/subsystems/SwerveModule.java | 89 ++++++++++++++----- .../java/frc/robot/subsystems/WriteLock.java | 44 +++++++++ 2 files changed, 109 insertions(+), 24 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/WriteLock.java diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 033bdd2..16023b3 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -4,6 +4,10 @@ package frc.robot.subsystems; +import java.util.concurrent.Executors; +import java.util.concurrent.ScheduledExecutorService; +import java.util.concurrent.TimeUnit; + import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.SensorInitializationStrategy; import com.ctre.phoenix.sensors.SensorTimeBase; @@ -41,6 +45,8 @@ public class SwerveModule extends SmartPrintable { private static final double ROCK_PID_D = 0.0; private final SwerveModulePosition position; + private final SwerveModuleRunner moduleRunner; + private final ScheduledExecutorService runnerThread; private final CANSparkMax rotationMotor; // The motor responsible for rotating the module. private final CANSparkMax movementMotor; // The motor responsible for creating movement in the module. @@ -56,6 +62,7 @@ public class SwerveModule extends SmartPrintable { private final Angle canCoderOffset; private SlewRateLimiter accelerationLimit; + private WriteLock desiredState; // Set to NaN if not in rock mode, NaN does not equal itself by definition // (see some IEEE standard or something) and so this is how rock mode is @@ -112,6 +119,53 @@ public static RelativePosition fromTranslation(Translation2d translation) { } } + private class SwerveModuleRunner implements Runnable { + private SwerveModule parentModule; + + public SwerveModuleRunner(SwerveModule parentModule) { + this.parentModule = parentModule; + } + + @Override + public void run() { + double currentPosition = (angularEncoder.getPosition() + canCoderOffset.radians()) % Angle.TAU; + + // Lock the parent module's desired state for reading. + SwerveModuleState parentState = parentModule.desiredState.lock(); + SwerveModuleState state = SwerveModuleState.optimize(parentState, new Rotation2d(currentPosition)); + + // Unlock to allow writing again. NOTE: the `parentState` variable + // MAY NOT BE USED beyond this point. + parentModule.desiredState.unlock(); + + /* + * There are two important reasons to run `SwerveModuleState.optomize()`. The first is obvious, make swerve + * more efficient. The other is to make a value-wise copy of the module state to avoid race conditions, as the + * method creates a new `SwerveModuleState` each time. See this link for source: + * https://github.wpilib.org/allwpilib/docs/release/java/src-html/edu/wpi/first/math/kinematics/SwerveModuleState.html#line.74 + */ + + if (rockPos != rockPos) { + var desiredSpeed = Math.abs(state.speedMetersPerSecond) > maxSpeed + ? Math.signum(state.speedMetersPerSecond) * maxSpeed + : state.speedMetersPerSecond; + movementMotor.set( + accelerationRate == accelerationRate + ? accelerationLimit.calculate(desiredSpeed) + : desiredSpeed + ); + } else { + movementMotor.set(rockController.calculate(getDistanceTraveled(), rockPos)); + } + + double calculation = rotationController.calculate(currentPosition, (state.angle.getRadians() + Angle.TAU) % Angle.TAU); + rotationMotor.set(calculation); + + position.angle = new Rotation2d(angularEncoder.getPosition()); + position.distanceMeters = movementEncoder.getPosition() * CONVERSION_FACTOR_MOVEMENT; + } + } + public SwerveModule( int movementMotorID, int rotationalMotorID, @@ -124,8 +178,6 @@ public SwerveModule( this.physicalPosition = RelativePosition.fromTranslation(physicalPosition); this.canCoderOffset = canCoderOffset.clone(); - accelerationLimit = new SlewRateLimiter(1.5); - rotationMotor = new CANSparkMax(rotationalMotorID, MotorType.kBrushless); rotationMotor.setInverted(true); rotationMotor.setSmartCurrentLimit(30); @@ -171,34 +223,23 @@ public SwerveModule( movementEncoder.getPosition() * CONVERSION_FACTOR_MOVEMENT, new Rotation2d(angularEncoder.getPosition()) ); + + accelerationLimit = new SlewRateLimiter(1.5); + desiredState = new WriteLock<>(new SwerveModuleState()); + moduleRunner = new SwerveModuleRunner(this); + + runnerThread = Executors.newSingleThreadScheduledExecutor(); + runnerThread.scheduleAtFixedRate(moduleRunner, 0, 20, TimeUnit.MILLISECONDS); } /** * Sets the desired module state for this module. This must be run * repeatedly to continue PID calculations. */ - public void setDesiredState(SwerveModuleState state) { - double currentPosition = (angularEncoder.getPosition() + canCoderOffset.radians()) % Angle.TAU; - state = SwerveModuleState.optimize(state, new Rotation2d(currentPosition)); - - if (rockPos != rockPos) { - var desiredSpeed = Math.abs(state.speedMetersPerSecond) > maxSpeed - ? Math.signum(state.speedMetersPerSecond) * maxSpeed - : state.speedMetersPerSecond; - movementMotor.set( - accelerationRate == accelerationRate - ? accelerationLimit.calculate(desiredSpeed) - : desiredSpeed - ); - } else { - movementMotor.set(rockController.calculate(getDistanceTraveled(), rockPos)); - } - - double calculation = rotationController.calculate(currentPosition, (state.angle.getRadians() + Angle.TAU) % Angle.TAU); - rotationMotor.set(calculation); - - position.angle = new Rotation2d(angularEncoder.getPosition()); - position.distanceMeters = movementEncoder.getPosition() * CONVERSION_FACTOR_MOVEMENT; + public void setDesiredState(SwerveModuleState desiredState) { + SwerveModuleState state = this.desiredState.lock(); + state = desiredState; + this.desiredState.unlock(state); } /** diff --git a/src/main/java/frc/robot/subsystems/WriteLock.java b/src/main/java/frc/robot/subsystems/WriteLock.java new file mode 100644 index 0000000..ce5c6cd --- /dev/null +++ b/src/main/java/frc/robot/subsystems/WriteLock.java @@ -0,0 +1,44 @@ +package frc.robot.subsystems; + +/** + * A write lock to prevent race conditions. This lock does not actually + * guarantee the memory safety of the contained object, each method call + * requres that the programmer make a "promise" to follow its rules. + */ +public class WriteLock { + private T obj; + private boolean locked; + + /** + * Creates a new lock around the given object. The given object mey not be used + * again after being passed in here except through the lock for the lock to be + * effective. + */ + public WriteLock(T obj) { + this.obj = obj; + } + + /** + * Blockes the current thread until the object is made available. + */ + public T lock() { + while (locked == true) {} + locked = true; + return obj; + } + + /** + * Unocks the object, it should not be used after this call. + */ + public void unlock() { + locked = false; + } + + /** + * Unocks the object, it should not be used after this call. Also sets the + * lock object to a new reference. + */ + public void unlock(T obj) { + this.obj = obj; + } +} From 846b5e20f1e1175edf41d3574b029036f344979c Mon Sep 17 00:00:00 2001 From: Evan Overman Date: Tue, 10 Oct 2023 17:19:28 -0700 Subject: [PATCH 6/8] use locks in `Pigeon` --- src/main/java/frc/robot/subsystems/Angle.java | 7 +-- src/main/java/frc/robot/systems/Pigeon.java | 43 ++++++++++++++----- 2 files changed, 34 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Angle.java b/src/main/java/frc/robot/subsystems/Angle.java index 9e25539..d63c5f7 100644 --- a/src/main/java/frc/robot/subsystems/Angle.java +++ b/src/main/java/frc/robot/subsystems/Angle.java @@ -4,7 +4,7 @@ package frc.robot.subsystems; -public final class Angle { +public final class Angle implements Cloneable { public static final double TAU = Math.PI * 2; private double dec = 0.0; @@ -49,10 +49,7 @@ public Angle div(double scalar) { return this.mul(1 / scalar); } - /** - * Creates a new identical angle so that modification of the original will not - * effect the value returned from this function. - */ + @Override public Angle clone() { var ang = new Angle(); ang.dec = this.dec; diff --git a/src/main/java/frc/robot/systems/Pigeon.java b/src/main/java/frc/robot/systems/Pigeon.java index ece930e..309d7e9 100644 --- a/src/main/java/frc/robot/systems/Pigeon.java +++ b/src/main/java/frc/robot/systems/Pigeon.java @@ -16,6 +16,7 @@ import frc.robot.SmartPrintable; import frc.robot.subsystems.Angle; +import frc.robot.subsystems.WriteLock; /* * Manages the robot's pigeon. @@ -27,7 +28,7 @@ public class Pigeon extends SmartPrintable { private final Pigeon2 pigeon; private final ScheduledExecutorService changeRateThread; - private OrientationalChange changePerSecond; + private WriteLock changePerSecond; private Pigeon(int canID) { super(); @@ -56,7 +57,10 @@ public static void setFeildZero() { * Gets the change per second in the orientation of the Pigeon. */ public static OrientationalChange getChangePerSecond() { - return instance.changePerSecond; + OrientationalChange change = instance.changePerSecond.lock(); + OrientationalChange ownedChange = change.clone(); + instance.changePerSecond.unlock(); + return ownedChange; } /** @@ -99,7 +103,7 @@ public void print() { * Represents the change per second in orientation as gathered and * calculated from the Pigeon. */ - public static class OrientationalChange { + public static class OrientationalChange implements Cloneable { public final Angle yawPerSecond; public final Angle rollPerSecond; public final Angle pitchPerSecond; @@ -109,9 +113,18 @@ public static class OrientationalChange { * references freely, withoutr modifying this class' state. */ private OrientationalChange(Angle yaw, Angle roll, Angle pitch) { - this.yawPerSecond = yaw.clone(); - this.rollPerSecond = roll.clone(); - this.pitchPerSecond = pitch.clone(); + yawPerSecond = yaw.clone(); + rollPerSecond = roll.clone(); + pitchPerSecond = pitch.clone(); + } + + @Override + public OrientationalChange clone() { + return new OrientationalChange( + yawPerSecond.clone(), + rollPerSecond.clone(), + pitchPerSecond.clone() + ); } } @@ -151,11 +164,19 @@ public void run() { double differenceSeconds = (double)(recordedInstant.toEpochMilli() - previousInstant.toEpochMilli()) / 1000.0; - Angle changeYaw = yaw.sub(previousYaw).div(differenceSeconds); - Angle changeRoll = roll.sub(previousRoll).div(differenceSeconds); - Angle changePitch = pitch.sub(previousPitch).div(differenceSeconds); - - pigeon.changePerSecond = new OrientationalChange(changeYaw, changeRoll, changePitch); + Angle changeYaw = yaw + .sub(previousYaw) + .div(differenceSeconds); + Angle changeRoll = roll + .sub(previousRoll) + .div(differenceSeconds); + Angle changePitch = pitch + .sub(previousPitch) + .div(differenceSeconds); + + pigeon.changePerSecond.lock(); + OrientationalChange change = new OrientationalChange(changeYaw, changeRoll, changePitch); + pigeon.changePerSecond.unlock(change); previousYaw = yaw; previousRoll = roll; From a93209e7d34462b0c4ca9cdca229ce3cdd984278 Mon Sep 17 00:00:00 2001 From: JustBeChill Date: Thu, 12 Oct 2023 16:41:48 -0700 Subject: [PATCH 7/8] huh --- src/main/java/frc/robot/Robot.java | 482 ++++---- src/main/java/frc/robot/subsystems/Angle.java | 126 +- .../frc/robot/subsystems/SwerveModule.java | 742 ++++++------ .../java/frc/robot/subsystems/WriteLock.java | 88 +- .../java/frc/robot/systems/AutoBalance.java | 364 +++--- src/main/java/frc/robot/systems/Pigeon.java | 378 +++--- .../java/frc/robot/systems/SwerveDrive.java | 1016 ++++++++--------- 7 files changed, 1601 insertions(+), 1595 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index dfab6cf..1dc13d2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,241 +1,241 @@ -// 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 frc.robot; - -import edu.wpi.first.wpilibj.AnalogInput; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.GenericHID.RumbleType; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -import frc.robot.systems.AutoBalance; -import frc.robot.systems.AutonomousRunner; -import frc.robot.systems.Controls; -import frc.robot.systems.ElevFourbar; -import frc.robot.systems.Intake; -import frc.robot.systems.Pigeon; -import frc.robot.systems.SmartPrinter; -import frc.robot.systems.SwerveDrive; -import frc.robot.systems.AutoBalance.Stage; -import frc.robot.systems.LEDLights; -import frc.robot.systems.Bat; -import frc.robot.subsystems.SwerveMode; - -/** -* The VM is configured to automatically run this class, and to call the functions corresponding to -* each mode, as described in the TimedRobot documentation. If you change the name of this class or -* the package after creating this project, you must also update the build.gradle file in the -* project. -*/ -public class Robot extends TimedRobot { - public enum ControlMode { - DISABLED, - AUTONOMOUS, - TELEOPERATED, - SAFETY - } - - private static final double DRIVE_ACCELERATION_LIMIT = 1.5; - - private static final XboxController controllerOne = (XboxController)Controls.getControllerByPort(0); - private static final XboxController controllerTwo = (XboxController)Controls.getControllerByPort(1); - private static final Joystick controlPanel = (Joystick)Controls.getControllerByPort(2); - - private static final AnalogInput pressureSensor = new AnalogInput(0); - private static final DigitalInput brakeSwitch = new DigitalInput(1); - - private static Robot instance; - - private ControlMode controlMode = ControlMode.DISABLED; - - /** - * Exists only to enable static methods to gain access to non static data, - * OOP fans be damned I just made your class a singleton. - */ - public Robot() { - super(); - instance = this; - } - - public static ControlMode getControlMode() { - return instance.controlMode; - } - - /** - * This function is run when the robot is first started up and should be used for any - * initialization code. - */ - @Override - public void robotInit() { - controlMode = ControlMode.DISABLED; - SwerveDrive.setRotationCurve(Controls::defaultCurve); - SwerveDrive.setTranslationCurve(Controls::defaultCurveTwoDimensional); - } - - /** - * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics - * that you want ran during disabled, autonomous, teleoperated and test. - * - *

This runs after the mode specific periodic functions, but before LiveWindow and - * SmartDashboard integrated updating. - */ - @Override - public void robotPeriodic() { - ElevFourbar.setFourbarBrake(brakeSwitch.get()); - double pressureValue = (pressureSensor.getValue() - 410) / 13.5; - - SmartDashboard.putNumber("Comp Pressure", Math.floor(pressureValue)); - SmartDashboard.putBoolean("Fully Pressurized", pressureValue > 60); - - SmartPrinter.print(); - LEDLights.run(); - Bat.getRange(); - } - - /** - * This autonomous (along with the chooser code above) shows how to select between different - * autonomous modes using the dashboard. The sendable chooser code works with the Java - * SmartDashboard. If you prefer the LabVIEW Dashboard, remove all of the chooser code and - * uncomment the getString line to get the auto name from the text box below the Gyro - * - *

You can add additional auto modes by adding additional comparisons to the switch structure - * below with additional strings. If using the SendableChooser make sure to add them to the - * chooser code above as well. - */ - @Override - public void autonomousInit() { - controlMode = ControlMode.AUTONOMOUS; - - int autoMode = - (controlPanel.getRawButton(12) ? 1 << 0 : 0) + - (controlPanel.getRawButton(11) ? 1 << 1 : 0) + - (controlPanel.getRawButton(10) ? 1 << 2 : 0); - - AutonomousRunner.init(); - AutonomousRunner.setAutoMode(autoMode); - - Pigeon.setFeildZero(); - SwerveDrive.zeroPositions(); - SwerveDrive.setAccelerationRate(Double.NaN); - //ElevFourbar.autonomousInit(); - ElevFourbar.init(); - Intake.init(); - - // Reset Auto Balance to the idling stage in case autonomous has been - // run more than once since code start. - AutoBalance.setStage(Stage.IDLING); - } - - /** This function is called periodically during autonomous. */ - @Override - public void autonomousPeriodic() { - AutonomousRunner.run(); - } - - /** This function is called once when teleop is enabled. */ - @Override - public void teleopInit() { - controlMode = ControlMode.TELEOPERATED; - SwerveDrive.setMode(SwerveMode.HEADLESS); - SwerveDrive.setAccelerationRate(Double.NaN); - ElevFourbar.init(); - Intake.init(); - } - - /** This function is called periodically during operator control. */ - @Override - public void teleopPeriodic() { - // Left stick changes between headless and relative control modes. - if (controllerOne.getLeftStickButtonReleased()) { - SwerveDrive.setMode( - SwerveDrive.getMode() == SwerveMode.HEADLESS - ? SwerveMode.RELATIVE - : SwerveMode.HEADLESS - ); - } - - if (controllerOne.getRightStickButtonReleased()) { - var accelerationRate = SwerveDrive.getAccelerationRate(); - SwerveDrive.setAccelerationRate( - accelerationRate == accelerationRate - ? Double.NaN - : DRIVE_ACCELERATION_LIMIT - ); - } - - SwerveDrive.conditionalTempTranslationCurve( - Controls.cardinalLock(Controls::defaultCurveTwoDimensional), - controllerOne.getXButton() - ); // Lock to cardinal directions. - SwerveDrive.conditionalTempTranslationCurve( - (x, y) -> Controls.defaultCurveTwoDimensional(x, y) / 3.0, - controllerOne.getRightBumper() - ); // Half translation speed. - SwerveDrive.conditionalTempTranslationCurve( - Controls.cardinalLock((x, y) -> Controls.defaultCurveTwoDimensional(x, y) / 2.0), - controllerOne.getRightBumper() && controllerOne.getXButton() - ); // Half translation speed. - SwerveDrive.conditionalTempMode(SwerveMode.ROCK, controllerOne.getBButton()); - SwerveDrive.run( - controllerOne.getLeftX(), - controllerOne.getLeftY(), - //controllerOne.getRightTriggerAxis() - controllerOne.getLeftTriggerAxis(), - controllerOne.getRightX() - ); - - double rumble = controllerOne.getLeftBumper() - ? SwerveDrive.getAverageMotorTemp() / 80.0 - : SwerveDrive.getAveragePercentRatedCurrent(); - controllerOne.setRumble(RumbleType.kBothRumble, rumble); - - Intake.run( - controlPanel.getRawButtonPressed(8), //Pivot toggle - controlPanel.getRawButton(9), //Claw hold - controlPanel.getRawButton(6), //Intake hold - controlPanel.getRawButton(7) //Outtake hold - ); - - ElevFourbar.run( - controllerTwo.getRightY(), //Manual elevator - Math.abs(controlPanel.getRawAxis(0) / 2) > Math.abs(controllerTwo.getLeftY()) ? controlPanel.getRawAxis(0) / 2 : -controllerTwo.getLeftY(), //Manual fourbar - controllerTwo.getPOV(), //DPad direction - controlPanel.getRawButtonPressed(5), //Toggle game piece - controlPanel.getRawButton(1), //Ground intake - controlPanel.getRawButton(3), //Mid scoring - controlPanel.getRawButton(4), //High scoring - controllerTwo.getStartButtonPressed() || controlPanel.getRawButton(2) //Zero elevator encoder - ); - } - - /** This function is called once when the robot is disabled. */ - @Override - public void disabledInit() { - controlMode = ControlMode.DISABLED; - } - - /** This function is called periodically when disabled. */ - @Override - public void disabledPeriodic() { - ElevFourbar.toggleGamePiece(controlPanel.getRawButtonReleased(5)); - } - - /** This function is called once when test mode is enabled. */ - @Override - public void testInit() {} - - /** This function is called periodically during test mode. */ - @Override - public void testPeriodic() {} - - /** This function is called once when the robot is first started up. */ - @Override - public void simulationInit() {} - - /** This function is called periodically whilst in simulation. */ - @Override - public void simulationPeriodic() {} -} +// 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 frc.robot; + +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import frc.robot.systems.AutoBalance; +import frc.robot.systems.AutonomousRunner; +import frc.robot.systems.Controls; +import frc.robot.systems.ElevFourbar; +import frc.robot.systems.Intake; +import frc.robot.systems.Pigeon; +import frc.robot.systems.SmartPrinter; +import frc.robot.systems.SwerveDrive; +import frc.robot.systems.AutoBalance.Stage; +import frc.robot.systems.LEDLights; +import frc.robot.systems.Bat; +import frc.robot.subsystems.SwerveMode; + +/** +* The VM is configured to automatically run this class, and to call the functions corresponding to +* each mode, as described in the TimedRobot documentation. If you change the name of this class or +* the package after creating this project, you must also update the build.gradle file in the +* project. +*/ +public class Robot extends TimedRobot { + public enum ControlMode { + DISABLED, + AUTONOMOUS, + TELEOPERATED, + SAFETY + } + + private static final double DRIVE_ACCELERATION_LIMIT = 1.5; + + private static final XboxController controllerOne = (XboxController)Controls.getControllerByPort(0); + private static final XboxController controllerTwo = (XboxController)Controls.getControllerByPort(1); + private static final Joystick controlPanel = (Joystick)Controls.getControllerByPort(2); + + private static final AnalogInput pressureSensor = new AnalogInput(0); + private static final DigitalInput brakeSwitch = new DigitalInput(1); + + private static Robot instance; + + private ControlMode controlMode = ControlMode.DISABLED; + + /** + * Exists only to enable static methods to gain access to non static data, + * OOP fans be damned I just made your class a singleton. + */ + public Robot() { + super(); + instance = this; + } + + public static ControlMode getControlMode() { + return instance.controlMode; + } + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + @Override + public void robotInit() { + controlMode = ControlMode.DISABLED; + SwerveDrive.setRotationCurve(Controls::defaultCurve); + SwerveDrive.setTranslationCurve(Controls::defaultCurveTwoDimensional); + } + + /** + * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics + * that you want ran during disabled, autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before LiveWindow and + * SmartDashboard integrated updating. + */ + @Override + public void robotPeriodic() { + ElevFourbar.setFourbarBrake(brakeSwitch.get()); + double pressureValue = (pressureSensor.getValue() - 410) / 13.5; + + SmartDashboard.putNumber("Comp Pressure", Math.floor(pressureValue)); + SmartDashboard.putBoolean("Fully Pressurized", pressureValue > 60); + + SmartPrinter.print(); + LEDLights.run(); + Bat.getRange(); + } + + /** + * This autonomous (along with the chooser code above) shows how to select between different + * autonomous modes using the dashboard. The sendable chooser code works with the Java + * SmartDashboard. If you prefer the LabVIEW Dashboard, remove all of the chooser code and + * uncomment the getString line to get the auto name from the text box below the Gyro + * + *

You can add additional auto modes by adding additional comparisons to the switch structure + * below with additional strings. If using the SendableChooser make sure to add them to the + * chooser code above as well. + */ + @Override + public void autonomousInit() { + controlMode = ControlMode.AUTONOMOUS; + + int autoMode = + (controlPanel.getRawButton(12) ? 1 << 0 : 0) + + (controlPanel.getRawButton(11) ? 1 << 1 : 0) + + (controlPanel.getRawButton(10) ? 1 << 2 : 0); + + AutonomousRunner.init(); + AutonomousRunner.setAutoMode(autoMode); + + Pigeon.setFeildZero(); + SwerveDrive.zeroPositions(); + SwerveDrive.setAccelerationRate(Double.NaN); + //ElevFourbar.autonomousInit(); + ElevFourbar.init(); + Intake.init(); + + // Reset Auto Balance to the idling stage in case autonomous has been + // run more than once since code start. + AutoBalance.setStage(Stage.IDLING); + } + + /** This function is called periodically during autonomous. */ + @Override + public void autonomousPeriodic() { + AutonomousRunner.run(); + } + + /** This function is called once when teleop is enabled. */ + @Override + public void teleopInit() { + controlMode = ControlMode.TELEOPERATED; + SwerveDrive.setMode(SwerveMode.HEADLESS); + SwerveDrive.setAccelerationRate(Double.NaN); + ElevFourbar.init(); + Intake.init(); + } + + /** This function is called periodically during operator control. */ + @Override + public void teleopPeriodic() { + // Left stick changes between headless and relative control modes. + if (controllerOne.getLeftStickButtonReleased()) { + SwerveDrive.setMode( + SwerveDrive.getMode() == SwerveMode.HEADLESS + ? SwerveMode.RELATIVE + : SwerveMode.HEADLESS + ); + } + + if (controllerOne.getRightStickButtonReleased()) { + var accelerationRate = SwerveDrive.getAccelerationRate(); + SwerveDrive.setAccelerationRate( + accelerationRate == accelerationRate + ? Double.NaN + : DRIVE_ACCELERATION_LIMIT + ); + } + + SwerveDrive.conditionalTempTranslationCurve( + Controls.cardinalLock(Controls::defaultCurveTwoDimensional), + controllerOne.getXButton() + ); // Lock to cardinal directions. + SwerveDrive.conditionalTempTranslationCurve( + (x, y) -> Controls.defaultCurveTwoDimensional(x, y) / 3.0, + controllerOne.getRightBumper() + ); // Half translation speed. + SwerveDrive.conditionalTempTranslationCurve( + Controls.cardinalLock((x, y) -> Controls.defaultCurveTwoDimensional(x, y) / 2.0), + controllerOne.getRightBumper() && controllerOne.getXButton() + ); // Half translation speed. + SwerveDrive.conditionalTempMode(SwerveMode.ROCK, controllerOne.getBButton()); + SwerveDrive.run( + controllerOne.getLeftX(), + controllerOne.getLeftY(), + //controllerOne.getRightTriggerAxis() - controllerOne.getLeftTriggerAxis(), + controllerOne.getRightX() + ); + + double rumble = controllerOne.getLeftBumper() + ? SwerveDrive.getAverageMotorTemp() / 80.0 + : SwerveDrive.getAveragePercentRatedCurrent(); + controllerOne.setRumble(RumbleType.kBothRumble, rumble); + + Intake.run( + controlPanel.getRawButtonPressed(8), //Pivot toggle + controlPanel.getRawButton(9), //Claw hold + controlPanel.getRawButton(6), //Intake hold + controlPanel.getRawButton(7) //Outtake hold + ); + + ElevFourbar.run( + controllerTwo.getRightY(), //Manual elevator + Math.abs(controlPanel.getRawAxis(0) / 2) > Math.abs(controllerTwo.getLeftY()) ? controlPanel.getRawAxis(0) / 2 : -controllerTwo.getLeftY(), //Manual fourbar + controllerTwo.getPOV(), //DPad direction + controlPanel.getRawButtonPressed(5), //Toggle game piece + controlPanel.getRawButton(1), //Ground intake + controlPanel.getRawButton(3), //Mid scoring + controlPanel.getRawButton(4), //High scoring + controllerTwo.getStartButtonPressed() || controlPanel.getRawButton(2) //Zero elevator encoder + ); + } + + /** This function is called once when the robot is disabled. */ + @Override + public void disabledInit() { + controlMode = ControlMode.DISABLED; + } + + /** This function is called periodically when disabled. */ + @Override + public void disabledPeriodic() { + ElevFourbar.toggleGamePiece(controlPanel.getRawButtonReleased(5)); + } + + /** This function is called once when test mode is enabled. */ + @Override + public void testInit() {} + + /** This function is called periodically during test mode. */ + @Override + public void testPeriodic() {} + + /** This function is called once when the robot is first started up. */ + @Override + public void simulationInit() {} + + /** This function is called periodically whilst in simulation. */ + @Override + public void simulationPeriodic() {} +} diff --git a/src/main/java/frc/robot/subsystems/Angle.java b/src/main/java/frc/robot/subsystems/Angle.java index d63c5f7..e92329c 100644 --- a/src/main/java/frc/robot/subsystems/Angle.java +++ b/src/main/java/frc/robot/subsystems/Angle.java @@ -1,63 +1,63 @@ -// 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 frc.robot.subsystems; - -public final class Angle implements Cloneable { - public static final double TAU = Math.PI * 2; - - private double dec = 0.0; - - public double radians() { - return dec * TAU; - } - - public double degrees() { - return dec * 360.0; - } - - public Angle setRadians(double radians) { - dec = radians / TAU; - return this; - } - - public Angle setDegrees(double degrees) { - dec = degrees / 360.0; - return this; - } - - public Angle add(Angle other) { - var angle = new Angle(); - angle.dec = this.dec + other.dec; - return angle; - } - - public Angle sub(Angle other) { - var angle = new Angle(); - angle.dec = this.dec - other.dec; - return angle; - } - - public Angle mul(double scalar) { - var angle = new Angle(); - angle.dec = this.dec * scalar; - return angle; - } - - public Angle div(double scalar) { - return this.mul(1 / scalar); - } - - @Override - public Angle clone() { - var ang = new Angle(); - ang.dec = this.dec; - return ang; - } - - @Override - public String toString() { - return this.degrees() + "°"; - } -} +// 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 frc.robot.subsystems; + +public final class Angle implements Cloneable { + public static final double TAU = Math.PI * 2; + + private double dec = 0.0; + + public double radians() { + return dec * TAU; + } + + public double degrees() { + return dec * 360.0; + } + + public Angle setRadians(double radians) { + dec = radians / TAU; + return this; + } + + public Angle setDegrees(double degrees) { + dec = degrees / 360.0; + return this; + } + + public Angle add(Angle other) { + var angle = new Angle(); + angle.dec = this.dec + other.dec; + return angle; + } + + public Angle sub(Angle other) { + var angle = new Angle(); + angle.dec = this.dec - other.dec; + return angle; + } + + public Angle mul(double scalar) { + var angle = new Angle(); + angle.dec = this.dec * scalar; + return angle; + } + + public Angle div(double scalar) { + return this.mul(1 / scalar); + } + + @Override + public Angle clone() { + var ang = new Angle(); + ang.dec = this.dec; + return ang; + } + + @Override + public String toString() { + return this.degrees() + "°"; + } +} diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 16023b3..dfd9ddd 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -1,371 +1,371 @@ -// 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 frc.robot.subsystems; - -import java.util.concurrent.Executors; -import java.util.concurrent.ScheduledExecutorService; -import java.util.concurrent.TimeUnit; - -import com.ctre.phoenix.sensors.CANCoder; -import com.ctre.phoenix.sensors.SensorInitializationStrategy; -import com.ctre.phoenix.sensors.SensorTimeBase; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import com.revrobotics.RelativeEncoder; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -import frc.robot.SmartPrintable; - -/** - * Class for managing and manipulating a swerve module. - */ -public class SwerveModule extends SmartPrintable { - private static final double CONVERSION_FACTOR_ROTATION = Math.toRadians(150 / 7); // Rotations to radians. - private static final double CONVERSION_FACTOR_MOVEMENT = 6.75; // Rotations to meters. - private static final double CONVERSION_FACTOR_ROTATION_VELOCITY = CONVERSION_FACTOR_ROTATION * (1 / 60); // RPM to radians per second. - private static final double CONVERSION_FACTOR_MOVEMENT_VELOCITY = CONVERSION_FACTOR_MOVEMENT * (1 / 60); // RPM to meters per second. - private static final double CAN_SPARK_MAX_RATED_AMPS = 60.0; - - private static final double PID_P = 0.5; - private static final double PID_I = 0.0; - private static final double PID_D = 0.0; - - private static final double ROCK_PID_P = 0.05; - private static final double ROCK_PID_I = 0.0; - private static final double ROCK_PID_D = 0.0; - - private final SwerveModulePosition position; - private final SwerveModuleRunner moduleRunner; - private final ScheduledExecutorService runnerThread; - - private final CANSparkMax rotationMotor; // The motor responsible for rotating the module. - private final CANSparkMax movementMotor; // The motor responsible for creating movement in the module. - private final CANCoder angularEncoder; // Cancoder responsible for tracking the angle of the module. - - private final RelativeEncoder rotationEncoder; // Relative encoder for tracking rotational movement. - private final RelativeEncoder movementEncoder; // Relative encoder for tracking translational movement. - - private final PIDController rotationController; - private final PIDController rockController; - - private final RelativePosition physicalPosition; - private final Angle canCoderOffset; - - private SlewRateLimiter accelerationLimit; - private WriteLock desiredState; - - // Set to NaN if not in rock mode, NaN does not equal itself by definition - // (see some IEEE standard or something) and so this is how rock mode is - // checked. Max speed works the same way. - private double rockPos = Double.NaN; - private double maxSpeed = Double.NaN; - private double accelerationRate = Double.NaN; - - private enum RelativePosition { - FRONT_RIGHT ( 1.0, 1.0 ), - FRONT_LEFT ( -1.0, 1.0 ), - BACK_LEFT ( -1.0, -1.0 ), - BACK_RIGHT ( 1.0, -1.0 ); - - private boolean front = true; - private boolean right = true; - - RelativePosition(double x, double y) { - right = Math.signum(x) > 0.0; - front = Math.signum(y) > 0.0; - } - - public String asString() { - String str = ""; - - if (front) { - str += "Front "; - } else { - str += "Back "; - } - - if (right) { - str += "Right"; - } else { - str += "Left"; - } - - return str; - } - - public static RelativePosition fromTranslation(Translation2d translation) { - var x_sign = Math.signum(translation.getX()) > 0.0; - var y_sign = Math.signum(translation.getY()) > 0.0; - - if (x_sign && y_sign) { - return FRONT_RIGHT; - } else if (x_sign) { - return BACK_RIGHT; - } else if (y_sign) { - return FRONT_LEFT; - } - - return BACK_LEFT; - } - } - - private class SwerveModuleRunner implements Runnable { - private SwerveModule parentModule; - - public SwerveModuleRunner(SwerveModule parentModule) { - this.parentModule = parentModule; - } - - @Override - public void run() { - double currentPosition = (angularEncoder.getPosition() + canCoderOffset.radians()) % Angle.TAU; - - // Lock the parent module's desired state for reading. - SwerveModuleState parentState = parentModule.desiredState.lock(); - SwerveModuleState state = SwerveModuleState.optimize(parentState, new Rotation2d(currentPosition)); - - // Unlock to allow writing again. NOTE: the `parentState` variable - // MAY NOT BE USED beyond this point. - parentModule.desiredState.unlock(); - - /* - * There are two important reasons to run `SwerveModuleState.optomize()`. The first is obvious, make swerve - * more efficient. The other is to make a value-wise copy of the module state to avoid race conditions, as the - * method creates a new `SwerveModuleState` each time. See this link for source: - * https://github.wpilib.org/allwpilib/docs/release/java/src-html/edu/wpi/first/math/kinematics/SwerveModuleState.html#line.74 - */ - - if (rockPos != rockPos) { - var desiredSpeed = Math.abs(state.speedMetersPerSecond) > maxSpeed - ? Math.signum(state.speedMetersPerSecond) * maxSpeed - : state.speedMetersPerSecond; - movementMotor.set( - accelerationRate == accelerationRate - ? accelerationLimit.calculate(desiredSpeed) - : desiredSpeed - ); - } else { - movementMotor.set(rockController.calculate(getDistanceTraveled(), rockPos)); - } - - double calculation = rotationController.calculate(currentPosition, (state.angle.getRadians() + Angle.TAU) % Angle.TAU); - rotationMotor.set(calculation); - - position.angle = new Rotation2d(angularEncoder.getPosition()); - position.distanceMeters = movementEncoder.getPosition() * CONVERSION_FACTOR_MOVEMENT; - } - } - - public SwerveModule( - int movementMotorID, - int rotationalMotorID, - int canCoderID, - Angle canCoderOffset, - Translation2d physicalPosition - ) { - super(); - - this.physicalPosition = RelativePosition.fromTranslation(physicalPosition); - this.canCoderOffset = canCoderOffset.clone(); - - rotationMotor = new CANSparkMax(rotationalMotorID, MotorType.kBrushless); - rotationMotor.setInverted(true); - rotationMotor.setSmartCurrentLimit(30); - - movementMotor = new CANSparkMax(movementMotorID, MotorType.kBrushless); - movementMotor.setInverted(false); - movementMotor.setIdleMode(IdleMode.kBrake); - movementMotor.setSmartCurrentLimit(40); - - angularEncoder = new CANCoder(canCoderID); - angularEncoder.configFactoryDefault(); - - // Magic and forbidden config from the code orange wizards. Makes the - // encoder initialize to absolute. - angularEncoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition); - angularEncoder.configFeedbackCoefficient( - // Since the default coefficiant used for degrees is not - // particularly intuitive we just grab it and run a deg -> rad - // conversion on it. - Math.toRadians(angularEncoder.configGetFeedbackCoefficient()), - "rad", - SensorTimeBase.PerSecond - ); - - rotationEncoder = rotationMotor.getEncoder(); - rotationEncoder.setPosition(angularEncoder.getPosition()); - rotationEncoder.setPositionConversionFactor(CONVERSION_FACTOR_ROTATION); - rotationEncoder.setVelocityConversionFactor(CONVERSION_FACTOR_ROTATION_VELOCITY); - - movementEncoder = movementMotor.getEncoder(); - movementEncoder.setPosition(0.0); - movementEncoder.setPositionConversionFactor(CONVERSION_FACTOR_MOVEMENT); - movementEncoder.setVelocityConversionFactor(CONVERSION_FACTOR_MOVEMENT_VELOCITY); - - rotationController = new PIDController(PID_P, PID_I, PID_D); - rotationController.enableContinuousInput(0.0, Angle.TAU); - rotationController.setTolerance(0.01); - - rockController = new PIDController(ROCK_PID_P, ROCK_PID_I, ROCK_PID_D); - rockController.setTolerance(1); - - position = new SwerveModulePosition( - movementEncoder.getPosition() * CONVERSION_FACTOR_MOVEMENT, - new Rotation2d(angularEncoder.getPosition()) - ); - - accelerationLimit = new SlewRateLimiter(1.5); - desiredState = new WriteLock<>(new SwerveModuleState()); - moduleRunner = new SwerveModuleRunner(this); - - runnerThread = Executors.newSingleThreadScheduledExecutor(); - runnerThread.scheduleAtFixedRate(moduleRunner, 0, 20, TimeUnit.MILLISECONDS); - } - - /** - * Sets the desired module state for this module. This must be run - * repeatedly to continue PID calculations. - */ - public void setDesiredState(SwerveModuleState desiredState) { - SwerveModuleState state = this.desiredState.lock(); - state = desiredState; - this.desiredState.unlock(state); - } - - /** - * True if the module should be in rock mode. - */ - public void setRockMode(boolean shouldHold) { - if (!shouldHold) { - rockPos = Double.NaN; - } else if (rockPos != rockPos) { - rockPos = getDistanceTraveled(); - } - } - - /** - * True if in rock mode. - */ - public boolean inRockMode() { - return rockPos == rockPos; - } - - /** - * Sets this module's maximum movement speed. Use NaN for no limit. - */ - public void setMaxSpeed(double maxSpeed) { - this.maxSpeed = maxSpeed; - } - - /** - * Gets this module's maximum movement speed - */ - public double getMaxSpeed() { - return maxSpeed; - } - - /** - * Sets whether or not to limit the acceleration of the module. - */ - public void setAccelerationRate(double accelerationRate) { - if (accelerationRate != this.accelerationRate && accelerationRate == accelerationRate) { - accelerationLimit = new SlewRateLimiter(accelerationRate); - } - - this.accelerationRate = accelerationRate; - } - - /** - * Gets whether or not the drive is limiting its acceleration. - */ - public double getAccelerationRate() { - return accelerationRate; - } - - /** - * Gets distance traveled, should not be used for ablsolute distances as - * this function currently makes no guarantee as to the starting position - * of the module. (This can be mitigated if you zero positions, but it will - * interupt odometry). - */ - public double getDistanceTraveled() { - return movementEncoder.getPosition(); - } - - /** - * Gets the reported tempurature of the rotation motor in celsius. - */ - public double getRotationMotorTemp() { - return rotationMotor.getMotorTemperature(); - } - - /** - * Gets the reported tempurature of the movement motor in celsius. - */ - public double getMovementMotorTemp() { - return movementMotor.getMotorTemperature(); - } - - /** - * Gets the power being outputted by the rotation motor's controller in amps. - */ - public double getRotationMotorCurrent() { - return rotationMotor.getOutputCurrent(); - } - - /** - * Gets the power being outputted by the movement motor's controller in amps. - */ - public double getMovementMotorCurrent() { - return movementMotor.getOutputCurrent(); - } - - /** - * Gets the sum of all motor's current in amps. - */ - public double getAppliedCurrent() { - return getRotationMotorCurrent() + getMovementMotorCurrent(); - } - - /** - * Gets the percentage of the maximum rated amperage of the motor - * controllers currently being hit by the module. - */ - public double getPercentRatedCurrent() { - return getAppliedCurrent() / (2.0 * CAN_SPARK_MAX_RATED_AMPS); - } - - @Override - public void print() { - SmartDashboard.putNumber("Module " + physicalPosition.asString() + "(ids: " + movementMotor.getDeviceId() + ", " + rotationMotor.getDeviceId() + ", " + angularEncoder.getDeviceID() + ") Position", Math.toDegrees(angularEncoder.getPosition())); - SmartDashboard.putNumber("Module " + physicalPosition.asString() + "(ids: " + movementMotor.getDeviceId() + ", " + rotationMotor.getDeviceId() + ", " + angularEncoder.getDeviceID() + ") Position mod 360", Math.toDegrees(angularEncoder.getPosition() % Angle.TAU)); - SmartDashboard.putNumber("Module " + physicalPosition.asString() + "(ids: " + movementMotor.getDeviceId() + ", " + rotationMotor.getDeviceId() + ", " + angularEncoder.getDeviceID() + ") Position + off", Math.toDegrees(angularEncoder.getPosition() + canCoderOffset.radians())); - SmartDashboard.putNumber("Module " + physicalPosition.asString() + "(ids: " + movementMotor.getDeviceId() + ", " + rotationMotor.getDeviceId() + ", " + angularEncoder.getDeviceID() + ") Position + off mod 360", Math.toDegrees((angularEncoder.getPosition() + canCoderOffset.radians()) % Angle.TAU)); - SmartDashboard.putNumber("Module " + physicalPosition.asString() + "(ids: " + movementMotor.getDeviceId() + ", " + rotationMotor.getDeviceId() + ", " + angularEncoder.getDeviceID() + ") Position (Distance) ", movementEncoder.getPosition()); - SmartDashboard.putNumber("Module " + physicalPosition.asString() + "(ids: " + movementMotor.getDeviceId() + ", " + rotationMotor.getDeviceId() + ", " + angularEncoder.getDeviceID() + ") Movement Speed", movementMotor.get()); - } - - /** - * Gets the angle of the module. - */ - public SwerveModulePosition getPosition() { - return position; - } - - /** - * Sets movement position to zero, will mess up odometry. - */ - public void zeroPositions() { - movementEncoder.setPosition(0.0); - } -} +// 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 frc.robot.subsystems; + +import java.util.concurrent.Executors; +import java.util.concurrent.ScheduledExecutorService; +import java.util.concurrent.TimeUnit; + +import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix.sensors.SensorInitializationStrategy; +import com.ctre.phoenix.sensors.SensorTimeBase; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import com.revrobotics.RelativeEncoder; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import frc.robot.SmartPrintable; + +/** + * Class for managing and manipulating a swerve module. + */ +public class SwerveModule extends SmartPrintable { + private static final double CONVERSION_FACTOR_ROTATION = Math.toRadians(150 / 7); // Rotations to radians. + private static final double CONVERSION_FACTOR_MOVEMENT = 6.75; // Rotations to meters. + private static final double CONVERSION_FACTOR_ROTATION_VELOCITY = CONVERSION_FACTOR_ROTATION * (1 / 60); // RPM to radians per second. + private static final double CONVERSION_FACTOR_MOVEMENT_VELOCITY = CONVERSION_FACTOR_MOVEMENT * (1 / 60); // RPM to meters per second. + private static final double CAN_SPARK_MAX_RATED_AMPS = 60.0; + + private static final double PID_P = 0.5; + private static final double PID_I = 0.0; + private static final double PID_D = 0.0; + + private static final double ROCK_PID_P = 0.05; + private static final double ROCK_PID_I = 0.0; + private static final double ROCK_PID_D = 0.0; + + private final SwerveModulePosition position; + private final SwerveModuleRunner moduleRunner; + private final ScheduledExecutorService runnerThread; + + private final CANSparkMax rotationMotor; // The motor responsible for rotating the module. + private final CANSparkMax movementMotor; // The motor responsible for creating movement in the module. + private final CANCoder angularEncoder; // Cancoder responsible for tracking the angle of the module. + + private final RelativeEncoder rotationEncoder; // Relative encoder for tracking rotational movement. + private final RelativeEncoder movementEncoder; // Relative encoder for tracking translational movement. + + private final PIDController rotationController; + private final PIDController rockController; + + private final RelativePosition physicalPosition; + private final Angle canCoderOffset; + + private SlewRateLimiter accelerationLimit; + private WriteLock desiredState; + + // Set to NaN if not in rock mode, NaN does not equal itself by definition + // (see some IEEE standard or something) and so this is how rock mode is + // checked. Max speed works the same way. + private double rockPos = Double.NaN; + private double maxSpeed = Double.NaN; + private double accelerationRate = Double.NaN; + + private enum RelativePosition { + FRONT_RIGHT ( 1.0, 1.0 ), + FRONT_LEFT ( -1.0, 1.0 ), + BACK_LEFT ( -1.0, -1.0 ), + BACK_RIGHT ( 1.0, -1.0 ); + + private boolean front = true; + private boolean right = true; + + RelativePosition(double x, double y) { + right = Math.signum(x) > 0.0; + front = Math.signum(y) > 0.0; + } + + public String asString() { + String str = ""; + + if (front) { + str += "Front "; + } else { + str += "Back "; + } + + if (right) { + str += "Right"; + } else { + str += "Left"; + } + + return str; + } + + public static RelativePosition fromTranslation(Translation2d translation) { + var x_sign = Math.signum(translation.getX()) > 0.0; + var y_sign = Math.signum(translation.getY()) > 0.0; + + if (x_sign && y_sign) { + return FRONT_RIGHT; + } else if (x_sign) { + return BACK_RIGHT; + } else if (y_sign) { + return FRONT_LEFT; + } + + return BACK_LEFT; + } + } + + private class SwerveModuleRunner implements Runnable { + private SwerveModule parentModule; + + public SwerveModuleRunner(SwerveModule parentModule) { + this.parentModule = parentModule; + } + + @Override + public void run() { + double currentPosition = (angularEncoder.getPosition() + canCoderOffset.radians()) % Angle.TAU; + + // Lock the parent module's desired state for reading. + SwerveModuleState parentState = parentModule.desiredState.lock(); + SwerveModuleState state = SwerveModuleState.optimize(parentState, new Rotation2d(currentPosition)); + + // Unlock to allow writing again. NOTE: the `parentState` variable + // MAY NOT BE USED beyond this point. + parentModule.desiredState.unlock(); + + /* + * There are two important reasons to run `SwerveModuleState.optomize()`. The first is obvious, make swerve + * more efficient. The other is to make a value-wise copy of the module state to avoid race conditions, as the + * method creates a new `SwerveModuleState` each time. See this link for source: + * https://github.wpilib.org/allwpilib/docs/release/java/src-html/edu/wpi/first/math/kinematics/SwerveModuleState.html#line.74 + */ + + if (rockPos != rockPos) { + var desiredSpeed = Math.abs(state.speedMetersPerSecond) > maxSpeed + ? Math.signum(state.speedMetersPerSecond) * maxSpeed + : state.speedMetersPerSecond; + movementMotor.set( + accelerationRate == accelerationRate + ? accelerationLimit.calculate(desiredSpeed) + : desiredSpeed + ); + } else { + movementMotor.set(rockController.calculate(getDistanceTraveled(), rockPos)); + } + + double calculation = rotationController.calculate(currentPosition, (state.angle.getRadians() + Angle.TAU) % Angle.TAU); + rotationMotor.set(calculation); + + position.angle = new Rotation2d(angularEncoder.getPosition()); + position.distanceMeters = movementEncoder.getPosition() * CONVERSION_FACTOR_MOVEMENT; + } + } + + public SwerveModule( + int movementMotorID, + int rotationalMotorID, + int canCoderID, + Angle canCoderOffset, + Translation2d physicalPosition + ) { + super(); + + this.physicalPosition = RelativePosition.fromTranslation(physicalPosition); + this.canCoderOffset = canCoderOffset.clone(); + + rotationMotor = new CANSparkMax(rotationalMotorID, MotorType.kBrushless); + rotationMotor.setInverted(true); + rotationMotor.setSmartCurrentLimit(30); + + movementMotor = new CANSparkMax(movementMotorID, MotorType.kBrushless); + movementMotor.setInverted(false); + movementMotor.setIdleMode(IdleMode.kBrake); + movementMotor.setSmartCurrentLimit(40); + + angularEncoder = new CANCoder(canCoderID); + angularEncoder.configFactoryDefault(); + + // Magic and forbidden config from the code orange wizards. Makes the + // encoder initialize to absolute. + angularEncoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition); + angularEncoder.configFeedbackCoefficient( + // Since the default coefficiant used for degrees is not + // particularly intuitive we just grab it and run a deg -> rad + // conversion on it. + Math.toRadians(angularEncoder.configGetFeedbackCoefficient()), + "rad", + SensorTimeBase.PerSecond + ); + + rotationEncoder = rotationMotor.getEncoder(); + rotationEncoder.setPosition(angularEncoder.getPosition()); + rotationEncoder.setPositionConversionFactor(CONVERSION_FACTOR_ROTATION); + rotationEncoder.setVelocityConversionFactor(CONVERSION_FACTOR_ROTATION_VELOCITY); + + movementEncoder = movementMotor.getEncoder(); + movementEncoder.setPosition(0.0); + movementEncoder.setPositionConversionFactor(CONVERSION_FACTOR_MOVEMENT); + movementEncoder.setVelocityConversionFactor(CONVERSION_FACTOR_MOVEMENT_VELOCITY); + + rotationController = new PIDController(PID_P, PID_I, PID_D); + rotationController.enableContinuousInput(0.0, Angle.TAU); + rotationController.setTolerance(0.01); + + rockController = new PIDController(ROCK_PID_P, ROCK_PID_I, ROCK_PID_D); + rockController.setTolerance(1); + + position = new SwerveModulePosition( + movementEncoder.getPosition() * CONVERSION_FACTOR_MOVEMENT, + new Rotation2d(angularEncoder.getPosition()) + ); + + accelerationLimit = new SlewRateLimiter(1.5); + desiredState = new WriteLock<>(new SwerveModuleState()); + moduleRunner = new SwerveModuleRunner(this); + + runnerThread = Executors.newSingleThreadScheduledExecutor(); + runnerThread.scheduleAtFixedRate(moduleRunner, 0, 20, TimeUnit.MILLISECONDS); + } + + /** + * Sets the desired module state for this module. This must be run + * repeatedly to continue PID calculations. + */ + public void setDesiredState(SwerveModuleState desiredState) { + SwerveModuleState state = this.desiredState.lock(); + state = desiredState; + this.desiredState.unlock(state); + } + + /** + * True if the module should be in rock mode. + */ + public void setRockMode(boolean shouldHold) { + if (!shouldHold) { + rockPos = Double.NaN; + } else if (rockPos != rockPos) { + rockPos = getDistanceTraveled(); + } + } + + /** + * True if in rock mode. + */ + public boolean inRockMode() { + return rockPos == rockPos; + } + + /** + * Sets this module's maximum movement speed. Use NaN for no limit. + */ + public void setMaxSpeed(double maxSpeed) { + this.maxSpeed = maxSpeed; + } + + /** + * Gets this module's maximum movement speed + */ + public double getMaxSpeed() { + return maxSpeed; + } + + /** + * Sets whether or not to limit the acceleration of the module. + */ + public void setAccelerationRate(double accelerationRate) { + if (accelerationRate != this.accelerationRate && accelerationRate == accelerationRate) { + accelerationLimit = new SlewRateLimiter(accelerationRate); + } + + this.accelerationRate = accelerationRate; + } + + /** + * Gets whether or not the drive is limiting its acceleration. + */ + public double getAccelerationRate() { + return accelerationRate; + } + + /** + * Gets distance traveled, should not be used for ablsolute distances as + * this function currently makes no guarantee as to the starting position + * of the module. (This can be mitigated if you zero positions, but it will + * interupt odometry). + */ + public double getDistanceTraveled() { + return movementEncoder.getPosition(); + } + + /** + * Gets the reported tempurature of the rotation motor in celsius. + */ + public double getRotationMotorTemp() { + return rotationMotor.getMotorTemperature(); + } + + /** + * Gets the reported tempurature of the movement motor in celsius. + */ + public double getMovementMotorTemp() { + return movementMotor.getMotorTemperature(); + } + + /** + * Gets the power being outputted by the rotation motor's controller in amps. + */ + public double getRotationMotorCurrent() { + return rotationMotor.getOutputCurrent(); + } + + /** + * Gets the power being outputted by the movement motor's controller in amps. + */ + public double getMovementMotorCurrent() { + return movementMotor.getOutputCurrent(); + } + + /** + * Gets the sum of all motor's current in amps. + */ + public double getAppliedCurrent() { + return getRotationMotorCurrent() + getMovementMotorCurrent(); + } + + /** + * Gets the percentage of the maximum rated amperage of the motor + * controllers currently being hit by the module. + */ + public double getPercentRatedCurrent() { + return getAppliedCurrent() / (2.0 * CAN_SPARK_MAX_RATED_AMPS); + } + + @Override + public void print() { + SmartDashboard.putNumber("Module " + physicalPosition.asString() + "(ids: " + movementMotor.getDeviceId() + ", " + rotationMotor.getDeviceId() + ", " + angularEncoder.getDeviceID() + ") Position", Math.toDegrees(angularEncoder.getPosition())); + SmartDashboard.putNumber("Module " + physicalPosition.asString() + "(ids: " + movementMotor.getDeviceId() + ", " + rotationMotor.getDeviceId() + ", " + angularEncoder.getDeviceID() + ") Position mod 360", Math.toDegrees(angularEncoder.getPosition() % Angle.TAU)); + SmartDashboard.putNumber("Module " + physicalPosition.asString() + "(ids: " + movementMotor.getDeviceId() + ", " + rotationMotor.getDeviceId() + ", " + angularEncoder.getDeviceID() + ") Position + off", Math.toDegrees(angularEncoder.getPosition() + canCoderOffset.radians())); + SmartDashboard.putNumber("Module " + physicalPosition.asString() + "(ids: " + movementMotor.getDeviceId() + ", " + rotationMotor.getDeviceId() + ", " + angularEncoder.getDeviceID() + ") Position + off mod 360", Math.toDegrees((angularEncoder.getPosition() + canCoderOffset.radians()) % Angle.TAU)); + SmartDashboard.putNumber("Module " + physicalPosition.asString() + "(ids: " + movementMotor.getDeviceId() + ", " + rotationMotor.getDeviceId() + ", " + angularEncoder.getDeviceID() + ") Position (Distance) ", movementEncoder.getPosition()); + SmartDashboard.putNumber("Module " + physicalPosition.asString() + "(ids: " + movementMotor.getDeviceId() + ", " + rotationMotor.getDeviceId() + ", " + angularEncoder.getDeviceID() + ") Movement Speed", movementMotor.get()); + } + + /** + * Gets the angle of the module. + */ + public SwerveModulePosition getPosition() { + return position; + } + + /** + * Sets movement position to zero, will mess up odometry. + */ + public void zeroPositions() { + movementEncoder.setPosition(0.0); + } +} diff --git a/src/main/java/frc/robot/subsystems/WriteLock.java b/src/main/java/frc/robot/subsystems/WriteLock.java index ce5c6cd..8fb299b 100644 --- a/src/main/java/frc/robot/subsystems/WriteLock.java +++ b/src/main/java/frc/robot/subsystems/WriteLock.java @@ -1,44 +1,44 @@ -package frc.robot.subsystems; - -/** - * A write lock to prevent race conditions. This lock does not actually - * guarantee the memory safety of the contained object, each method call - * requres that the programmer make a "promise" to follow its rules. - */ -public class WriteLock { - private T obj; - private boolean locked; - - /** - * Creates a new lock around the given object. The given object mey not be used - * again after being passed in here except through the lock for the lock to be - * effective. - */ - public WriteLock(T obj) { - this.obj = obj; - } - - /** - * Blockes the current thread until the object is made available. - */ - public T lock() { - while (locked == true) {} - locked = true; - return obj; - } - - /** - * Unocks the object, it should not be used after this call. - */ - public void unlock() { - locked = false; - } - - /** - * Unocks the object, it should not be used after this call. Also sets the - * lock object to a new reference. - */ - public void unlock(T obj) { - this.obj = obj; - } -} +package frc.robot.subsystems; + +/** + * A write lock to prevent race conditions. This lock does not actually + * guarantee the memory safety of the contained object, each method call + * requres that the programmer make a "promise" to follow its rules. + */ +public class WriteLock { + private T obj; + private boolean locked; + + /** + * Creates a new lock around the given object. The given object mey not be used + * again after being passed in here except through the lock for the lock to be + * effective. + */ + public WriteLock(T obj) { + this.obj = obj; + } + + /** + * Blockes the current thread until the object is made available. + */ + public T lock() { + while (locked == true) {} + locked = true; + return obj; + } + + /** + * Unocks the object, it should not be used after this call. + */ + public void unlock() { + locked = false; + } + + /** + * Unocks the object, it should not be used after this call. Also sets the + * lock object to a new reference. + */ + public void unlock(T obj) { + this.obj = obj; + } +} diff --git a/src/main/java/frc/robot/systems/AutoBalance.java b/src/main/java/frc/robot/systems/AutoBalance.java index 5578134..71e1be4 100644 --- a/src/main/java/frc/robot/systems/AutoBalance.java +++ b/src/main/java/frc/robot/systems/AutoBalance.java @@ -1,182 +1,182 @@ -package frc.robot.systems; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.SmartPrintable; - -/** - * Manages the SwerDrive class for auto balancing, this is only seperate since - * SwerveDrive was becoming a bit large to handle. - */ -public class AutoBalance extends SmartPrintable { - private static final double RAMMING_SPEED = -0.75; - private static final double HALTING_SPEED = 0.1; - - private static final AutoBalance instance = new AutoBalance(); - - private Stage stage; - private BalanceType type; - private boolean rammingSubStageTwo; - private Timer timer; - - public enum BalanceType { - OVER_AND_BACK, - DOCK - } - - public enum Stage { - /** - * auto balance is doing nothing. - */ - IDLING, - - /** - * auto balance is ramming, meant to dock the charge station. - */ - RAMMING, - - /** - * auto balance is on the charge station and trying to engage the charge - * station. - */ - ADJUSTING, - - /** - * auto balance has paused, either becaue it has finished, or because - * it has encountered a spike in angular change. - */ - PAUSED, - } - - private AutoBalance() { - super(); - stage = Stage.IDLING; - type = BalanceType.DOCK; - rammingSubStageTwo = false; - timer = new Timer(); - } - - /** - * Call in a loop during autonomous to auto balance, auto balance expects - * to be the last action taken during autonomous and other actions affecting - * the drive train should not be taken. - */ - public static void run() { - if (instance.stage == Stage.IDLING) { - instance.stage = Stage.RAMMING; - } - - switch (instance.stage) { - case RAMMING: - instance.stage = instance.ram(); - break; - - case ADJUSTING: - instance.stage = instance.adjust(); - break; - - case PAUSED: - instance.stage = instance.pause(); - break; - - default: - throw new RuntimeException("AutoBalance stage was misshandled. Value: " + instance.stage.toString()); - } - - instance.print(); - } - - /** - * Gets the auto balance stage. - */ - public static Stage getStage() { - return instance.stage; - } - - public static BalanceType getType() { - return instance.type; - } - - /** - * Manually sets the stage of auto balance. - */ - public static void setStage(Stage stage) { - instance.stage = stage; - - if (stage == Stage.IDLING) { - instance.rammingSubStageTwo = false; - } - } - - public static void setType(BalanceType type) { - instance.type = type; - } - - private Stage ram() { - if (Math.abs(SwerveDrive.getDistance()) > 365 && type == BalanceType.DOCK) { - return Stage.ADJUSTING; - } else if (Math.abs(SwerveDrive.getDistance()) > 365 + 330 + 30 && !rammingSubStageTwo) { - rammingSubStageTwo = true; - timer.reset(); - timer.start(); - SwerveDrive.zeroPositions(); - } else if (Math.abs(SwerveDrive.getDistance()) > 365 + 60 + 90 && rammingSubStageTwo && timer.get() > 1.0) { - return Stage.ADJUSTING; - } - - SwerveDrive.runUncurved( - 0.0, - RAMMING_SPEED - * (rammingSubStageTwo ? -1.0 : 1.0) - * (Math.abs(SwerveDrive.getDistance()) > 330 ? 0.5 : 1.0), - 0.0 - ); - - return Stage.RAMMING; - } - - private Stage adjust() { - if (level() || spiking()) { - return Stage.PAUSED; - } - - SwerveDrive.runUncurved(0.0, Math.signum(Pigeon.getPitch().degrees()) * HALTING_SPEED, 0.0); - return Stage.ADJUSTING; - } - - private Stage pause() { - if (!level() && !spiking()) { - return Stage.ADJUSTING; - } - - //SwerveDrive.setRockMode(true); - SwerveDrive.runUncurved(0.0, 0.0, 0.0); - //SwerveDrive.runRockMode(); - return Stage.PAUSED; - } - - /** - * Returns true if auto balance beleives it is balanced, not 100% accurate. - */ - private boolean level() { - return Math.abs(Pigeon.getPitch().degrees()) < 10.0 - && Math.abs(Pigeon.getChangePerSecond().pitchPerSecond.degrees()) < 10.0; - } - - /** - * Returns true if auto balance beleives it is spiking in angle. - */ - private boolean spiking() { - return Math.abs(Pigeon.getChangePerSecond().pitchPerSecond.degrees()) > 10.0; - } - - /** - * Print information about auto balance. - */ - @Override - public void print() { - SmartDashboard.putString("Auto Balance Stage", instance.stage.toString()); - SmartDashboard.putBoolean("Auto Balance Spiking", spiking()); - SmartDashboard.putBoolean("Auto Balance Balanced", level()); - } -} +package frc.robot.systems; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.SmartPrintable; + +/** + * Manages the SwerDrive class for auto balancing, this is only seperate since + * SwerveDrive was becoming a bit large to handle. + */ +public class AutoBalance extends SmartPrintable { + private static final double RAMMING_SPEED = -0.75; + private static final double HALTING_SPEED = 0.1; + + private static final AutoBalance instance = new AutoBalance(); + + private Stage stage; + private BalanceType type; + private boolean rammingSubStageTwo; + private Timer timer; + + public enum BalanceType { + OVER_AND_BACK, + DOCK + } + + public enum Stage { + /** + * auto balance is doing nothing. + */ + IDLING, + + /** + * auto balance is ramming, meant to dock the charge station. + */ + RAMMING, + + /** + * auto balance is on the charge station and trying to engage the charge + * station. + */ + ADJUSTING, + + /** + * auto balance has paused, either becaue it has finished, or because + * it has encountered a spike in angular change. + */ + PAUSED, + } + + private AutoBalance() { + super(); + stage = Stage.IDLING; + type = BalanceType.DOCK; + rammingSubStageTwo = false; + timer = new Timer(); + } + + /** + * Call in a loop during autonomous to auto balance, auto balance expects + * to be the last action taken during autonomous and other actions affecting + * the drive train should not be taken. + */ + public static void run() { + if (instance.stage == Stage.IDLING) { + instance.stage = Stage.RAMMING; + } + + switch (instance.stage) { + case RAMMING: + instance.stage = instance.ram(); + break; + + case ADJUSTING: + instance.stage = instance.adjust(); + break; + + case PAUSED: + instance.stage = instance.pause(); + break; + + default: + throw new RuntimeException("AutoBalance stage was misshandled. Value: " + instance.stage.toString()); + } + + instance.print(); + } + + /** + * Gets the auto balance stage. + */ + public static Stage getStage() { + return instance.stage; + } + + public static BalanceType getType() { + return instance.type; + } + + /** + * Manually sets the stage of auto balance. + */ + public static void setStage(Stage stage) { + instance.stage = stage; + + if (stage == Stage.IDLING) { + instance.rammingSubStageTwo = false; + } + } + + public static void setType(BalanceType type) { + instance.type = type; + } + + private Stage ram() { + if (Math.abs(SwerveDrive.getDistance()) > 365 && type == BalanceType.DOCK) { + return Stage.ADJUSTING; + } else if (Math.abs(SwerveDrive.getDistance()) > 365 + 330 + 30 && !rammingSubStageTwo) { + rammingSubStageTwo = true; + timer.reset(); + timer.start(); + SwerveDrive.zeroPositions(); + } else if (Math.abs(SwerveDrive.getDistance()) > 365 + 60 + 90 && rammingSubStageTwo && timer.get() > 1.0) { + return Stage.ADJUSTING; + } + + SwerveDrive.runUncurved( + 0.0, + RAMMING_SPEED + * (rammingSubStageTwo ? -1.0 : 1.0) + * (Math.abs(SwerveDrive.getDistance()) > 330 ? 0.5 : 1.0), + 0.0 + ); + + return Stage.RAMMING; + } + + private Stage adjust() { + if (level() || spiking()) { + return Stage.PAUSED; + } + + SwerveDrive.runUncurved(0.0, Math.signum(Pigeon.getPitch().degrees()) * HALTING_SPEED, 0.0); + return Stage.ADJUSTING; + } + + private Stage pause() { + if (!level() && !spiking()) { + return Stage.ADJUSTING; + } + + //SwerveDrive.setRockMode(true); + SwerveDrive.runUncurved(0.0, 0.0, 0.0); + //SwerveDrive.runRockMode(); + return Stage.PAUSED; + } + + /** + * Returns true if auto balance beleives it is balanced, not 100% accurate. + */ + private boolean level() { + return Math.abs(Pigeon.getPitch().degrees()) < 10.0 + && Math.abs(Pigeon.getChangePerSecond().pitchPerSecond.degrees()) < 10.0; + } + + /** + * Returns true if auto balance beleives it is spiking in angle. + */ + private boolean spiking() { + return Math.abs(Pigeon.getChangePerSecond().pitchPerSecond.degrees()) > 10.0; + } + + /** + * Print information about auto balance. + */ + @Override + public void print() { + SmartDashboard.putString("Auto Balance Stage", instance.stage.toString()); + SmartDashboard.putBoolean("Auto Balance Spiking", spiking()); + SmartDashboard.putBoolean("Auto Balance Balanced", level()); + } +} diff --git a/src/main/java/frc/robot/systems/Pigeon.java b/src/main/java/frc/robot/systems/Pigeon.java index 309d7e9..35a689c 100644 --- a/src/main/java/frc/robot/systems/Pigeon.java +++ b/src/main/java/frc/robot/systems/Pigeon.java @@ -1,186 +1,192 @@ -// 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 frc.robot.systems; - -import java.time.Clock; -import java.time.Instant; -import java.util.concurrent.Executors; -import java.util.concurrent.ScheduledExecutorService; -import java.util.concurrent.TimeUnit; - -import com.ctre.phoenix.sensors.Pigeon2; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -import frc.robot.SmartPrintable; -import frc.robot.subsystems.Angle; -import frc.robot.subsystems.WriteLock; - -/* - * Manages the robot's pigeon. - */ -public class Pigeon extends SmartPrintable { - private static final int PIGEON_CAN_ID = 20; - private static final Pigeon instance = new Pigeon(PIGEON_CAN_ID); - - private final Pigeon2 pigeon; - private final ScheduledExecutorService changeRateThread; - - private WriteLock changePerSecond; - - private Pigeon(int canID) { - super(); - pigeon = new Pigeon2(canID); - - /* - * Starts the thread so that it calls 'run()' every 40 ms (25hz). This - * automatically updates the changePerSecond feilds at that rate. - */ - - OrientationalChangeCalculator angularChangeCalculator = new OrientationalChangeCalculator(this); - changeRateThread = Executors.newSingleThreadScheduledExecutor(); - changeRateThread.scheduleAtFixedRate(angularChangeCalculator, 0, 40, TimeUnit.MILLISECONDS); - } - - /** - * Sets "relative forward" to the current position. This will make getting - * the relative rotation always return a rotation relative to the rotation - * the robot is in at the point this method is called. - */ - public static void setFeildZero() { - instance.pigeon.setYaw(0.0); - } - - /** - * Gets the change per second in the orientation of the Pigeon. - */ - public static OrientationalChange getChangePerSecond() { - OrientationalChange change = instance.changePerSecond.lock(); - OrientationalChange ownedChange = change.clone(); - instance.changePerSecond.unlock(); - return ownedChange; - } - - /** - * Gets Yaw. - */ - public static Angle getYaw() { - return new Angle().setDegrees(instance.pigeon.getYaw() % 360.0); - } - - /** - * Gets pitch. - */ - public static Angle getPitch() { - // Pitch and roll are flipped due to mounting orientation. - return new Angle().setDegrees(instance.pigeon.getRoll()); - } - - /** - * Gets roll. - */ - public static Angle getRoll() { - // Pitch and roll are flipped due to mounting orientation. - return new Angle().setDegrees(instance.pigeon.getPitch()); - } - - @Override - public void print() { - SmartDashboard.putString("Pigeon Yaw", getYaw().toString()); - SmartDashboard.putString("Pigeon Pitch", getPitch().toString()); - SmartDashboard.putString("Pigeon Roll", getRoll().toString()); - - OrientationalChange change = getChangePerSecond(); - - SmartDashboard.putString("Pigeon Yaw/Sec", change.yawPerSecond.toString()); - SmartDashboard.putString("Pigeon Pitch/Sec", change.pitchPerSecond.toString()); - SmartDashboard.putString("Pigeon Roll/Sec", change.rollPerSecond.toString()); - } - - /** - * Represents the change per second in orientation as gathered and - * calculated from the Pigeon. - */ - public static class OrientationalChange implements Cloneable { - public final Angle yawPerSecond; - public final Angle rollPerSecond; - public final Angle pitchPerSecond; - - /** - * Clones all given angles allowing the caller to mutate the passed in - * references freely, withoutr modifying this class' state. - */ - private OrientationalChange(Angle yaw, Angle roll, Angle pitch) { - yawPerSecond = yaw.clone(); - rollPerSecond = roll.clone(); - pitchPerSecond = pitch.clone(); - } - - @Override - public OrientationalChange clone() { - return new OrientationalChange( - yawPerSecond.clone(), - rollPerSecond.clone(), - pitchPerSecond.clone() - ); - } - } - - /* - * This is made a subclass to make sure no one gets confused and tries to - * run the whole Pigeon on a thread in Robot.java - */ - private class OrientationalChangeCalculator implements Runnable { - private final Pigeon pigeon; // Reference to containing pigeon. - private final Clock clock; // Clock used to get current instant. - - private Instant recordedInstant; - - private Angle previousYaw; - private Angle previousRoll; - private Angle previousPitch; - - private OrientationalChangeCalculator(Pigeon pigeon) { - this.pigeon = pigeon; - clock = Clock.systemDefaultZone(); - recordedInstant = clock.instant(); - - previousYaw = new Angle().setDegrees(pigeon.pigeon.getYaw()); - previousRoll = new Angle().setDegrees(pigeon.pigeon.getPitch()); // Roll and Pitch are swapped cause of the way its mounted. - previousPitch = new Angle().setDegrees(pigeon.pigeon.getRoll()); - } - - @Override - public void run() { - Instant previousInstant = recordedInstant; - - Angle yaw = new Angle().setDegrees(pigeon.pigeon.getYaw()); - Angle roll = new Angle().setDegrees(pigeon.pigeon.getPitch()); // Roll and Pitch are swapped cause of the way its mounted. - Angle pitch = new Angle().setDegrees(pigeon.pigeon.getRoll()); - - recordedInstant = clock.instant(); - - double differenceSeconds = (double)(recordedInstant.toEpochMilli() - previousInstant.toEpochMilli()) / 1000.0; - - Angle changeYaw = yaw - .sub(previousYaw) - .div(differenceSeconds); - Angle changeRoll = roll - .sub(previousRoll) - .div(differenceSeconds); - Angle changePitch = pitch - .sub(previousPitch) - .div(differenceSeconds); - - pigeon.changePerSecond.lock(); - OrientationalChange change = new OrientationalChange(changeYaw, changeRoll, changePitch); - pigeon.changePerSecond.unlock(change); - - previousYaw = yaw; - previousRoll = roll; - previousPitch = pitch; - } - } -} + // 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 frc.robot.systems; + +import java.time.Clock; +import java.time.Instant; +import java.util.concurrent.Executors; +import java.util.concurrent.ScheduledExecutorService; +import java.util.concurrent.TimeUnit; + +import com.ctre.phoenix.sensors.Pigeon2; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import frc.robot.SmartPrintable; +import frc.robot.subsystems.Angle; +import frc.robot.subsystems.WriteLock; + +/* + * Manages the robot's pigeon. + */ +public class Pigeon extends SmartPrintable { + private static final int PIGEON_CAN_ID = 20; + private static final Pigeon instance = new Pigeon(PIGEON_CAN_ID); + + private final Pigeon2 pigeon; + private final ScheduledExecutorService changeRateThread; + + private WriteLock changePerSecond; + + private Pigeon(int canID) { + super(); + pigeon = new Pigeon2(canID); + + changePerSecond = new WriteLock(new OrientationalChange( + new Angle().setDegrees(0.0), + new Angle().setDegrees(0.0), + new Angle().setDegrees(0.0) + )); + + /* + * Starts the thread so that it calls 'run()' every 40 ms (25hz). This + * automatically updates the changePerSecond feilds at that rate. + */ + + OrientationalChangeCalculator angularChangeCalculator = new OrientationalChangeCalculator(this); + changeRateThread = Executors.newSingleThreadScheduledExecutor(); + changeRateThread.scheduleAtFixedRate(angularChangeCalculator, 0, 40, TimeUnit.MILLISECONDS); + } + + /** + * Sets "relative forward" to the current position. This will make getting + * the relative rotation always return a rotation relative to the rotation + * the robot is in at the point this method is called. + */ + public static void setFeildZero() { + instance.pigeon.setYaw(0.0); + } + + /** + * Gets the change per second in the orientation of the Pigeon. + */ + public static OrientationalChange getChangePerSecond() { + OrientationalChange change = instance.changePerSecond.lock(); + OrientationalChange ownedChange = change.clone(); + instance.changePerSecond.unlock(); + return ownedChange; + } + + /** + * Gets Yaw. + */ + public static Angle getYaw() { + return new Angle().setDegrees(instance.pigeon.getYaw() % 360.0); + } + + /** + * Gets pitch. + */ + public static Angle getPitch() { + // Pitch and roll are flipped due to mounting orientation. + return new Angle().setDegrees(instance.pigeon.getRoll()); + } + + /** + * Gets roll. + */ + public static Angle getRoll() { + // Pitch and roll are flipped due to mounting orientation. + return new Angle().setDegrees(instance.pigeon.getPitch()); + } + + @Override + public void print() { + SmartDashboard.putString("Pigeon Yaw", getYaw().toString()); + SmartDashboard.putString("Pigeon Pitch", getPitch().toString()); + SmartDashboard.putString("Pigeon Roll", getRoll().toString()); + + OrientationalChange change = getChangePerSecond(); + + SmartDashboard.putString("Pigeon Yaw/Sec", change.yawPerSecond.toString()); + SmartDashboard.putString("Pigeon Pitch/Sec", change.pitchPerSecond.toString()); + SmartDashboard.putString("Pigeon Roll/Sec", change.rollPerSecond.toString()); + } + + /** + * Represents the change per second in orientation as gathered and + * calculated from the Pigeon. + */ + public static class OrientationalChange implements Cloneable { + public final Angle yawPerSecond; + public final Angle rollPerSecond; + public final Angle pitchPerSecond; + + /** + * Clones all given angles allowing the caller to mutate the passed in + * references freely, withoutr modifying this class' state. + */ + private OrientationalChange(Angle yaw, Angle roll, Angle pitch) { + yawPerSecond = yaw.clone(); + rollPerSecond = roll.clone(); + pitchPerSecond = pitch.clone(); + } + + @Override + public OrientationalChange clone() { + return new OrientationalChange( + yawPerSecond.clone(), + rollPerSecond.clone(), + pitchPerSecond.clone() + ); + } + } + + /* + * This is made a subclass to make sure no one gets confused and tries to + * run the whole Pigeon on a thread in Robot.java + */ + private class OrientationalChangeCalculator implements Runnable { + private final Pigeon pigeon; // Reference to containing pigeon. + private final Clock clock; // Clock used to get current instant. + + private Instant recordedInstant; + + private Angle previousYaw; + private Angle previousRoll; + private Angle previousPitch; + + private OrientationalChangeCalculator(Pigeon pigeon) { + this.pigeon = pigeon; + clock = Clock.systemDefaultZone(); + recordedInstant = clock.instant(); + + previousYaw = new Angle().setDegrees(pigeon.pigeon.getYaw()); + previousRoll = new Angle().setDegrees(pigeon.pigeon.getPitch()); // Roll and Pitch are swapped cause of the way its mounted. + previousPitch = new Angle().setDegrees(pigeon.pigeon.getRoll()); + } + + @Override + public void run() { + Instant previousInstant = recordedInstant; + + Angle yaw = new Angle().setDegrees(pigeon.pigeon.getYaw()); + Angle roll = new Angle().setDegrees(pigeon.pigeon.getPitch()); // Roll and Pitch are swapped cause of the way its mounted. + Angle pitch = new Angle().setDegrees(pigeon.pigeon.getRoll()); + + recordedInstant = clock.instant(); + + double differenceSeconds = (double)(recordedInstant.toEpochMilli() - previousInstant.toEpochMilli()) / 1000.0; + + Angle changeYaw = yaw + .sub(previousYaw) + .div(differenceSeconds); + Angle changeRoll = roll + .sub(previousRoll) + .div(differenceSeconds); + Angle changePitch = pitch + .sub(previousPitch) + .div(differenceSeconds); + + pigeon.changePerSecond.lock(); + OrientationalChange change = new OrientationalChange(changeYaw, changeRoll, changePitch); + pigeon.changePerSecond.unlock(change); + + previousYaw = yaw; + previousRoll = roll; + previousPitch = pitch; + } + } +} diff --git a/src/main/java/frc/robot/systems/SwerveDrive.java b/src/main/java/frc/robot/systems/SwerveDrive.java index b89a40c..af2c98e 100644 --- a/src/main/java/frc/robot/systems/SwerveDrive.java +++ b/src/main/java/frc/robot/systems/SwerveDrive.java @@ -1,508 +1,508 @@ -// 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 frc.robot.systems; - -import java.util.function.BiFunction; -import java.util.function.Function; - -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.SwerveDriveOdometry; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.SmartPrintable; -import frc.robot.subsystems.Angle; -import frc.robot.subsystems.SwerveMode; -import frc.robot.subsystems.SwerveModule; - -/* - * Manages the swerve drive train. - */ -public class SwerveDrive extends SmartPrintable { - private static final int MODULE_MOVEMENT_CAN_IDS[] = { 1, 2, 3, 4 }; - private static final int MODULE_ROTATION_CAN_IDS[] = { 5, 6, 7, 8 }; - private static final int MODULE_CANCODER_CAN_IDS[] = { 9, 10, 11, 12 }; - - private static final double CHASSIS_SIDE_LENGTH = 0.6; - - private static final Angle MODULE_CANCODER_OFFSETS[] = { - new Angle().setDegrees(-252.24607237 + 90.0), - new Angle().setDegrees(-224.033203125 + 270.0), - new Angle().setDegrees(-11.425719246268272 + 270.0), - new Angle().setDegrees(-179.56050113588573 + 90.0) - }; - - private static final Angle MODULE_ROCK_MODE_POSITIONS[] = { - new Angle().setRadians( -Angle.TAU / 8 ), - new Angle().setRadians( Angle.TAU / 8 ), - new Angle().setRadians( -Angle.TAU / 8 ), - new Angle().setRadians( Angle.TAU / 8 ) - }; - - private static final Translation2d MODULE_PHYSICAL_POSITIONS[] = { - new Translation2d( CHASSIS_SIDE_LENGTH / 2, CHASSIS_SIDE_LENGTH / 2 ), - new Translation2d( -CHASSIS_SIDE_LENGTH / 2, CHASSIS_SIDE_LENGTH / 2 ), - new Translation2d( -CHASSIS_SIDE_LENGTH / 2, -CHASSIS_SIDE_LENGTH / 2 ), - new Translation2d( CHASSIS_SIDE_LENGTH / 2, -CHASSIS_SIDE_LENGTH / 2 ) - }; - - // Singleton instance. - private static final SwerveDrive instance = new SwerveDrive(); - - // Internally mutable state objects - private final SwerveModule modules[] = new SwerveModule[MODULE_MOVEMENT_CAN_IDS.length]; - private final SwerveModulePosition positions[] = new SwerveModulePosition[MODULE_MOVEMENT_CAN_IDS.length]; - private final SwerveDriveKinematics kinematics; - private final SwerveDriveOdometry odometry; - - // Fully mutable state objects - private BiFunction translationCurve = Controls::defaultCurveTwoDimensional; - private BiFunction inactiveTransationCurve = null; - private Function rotationCurve = Controls::defaultCurve; - private Function inactiveRotationCurve = null; - private SwerveMode mode = SwerveMode.HEADLESS; - private SwerveMode inactiveMode = null; - private SwerveMode displayMode = SwerveMode.HEADLESS; - - private double translationSpeedX = 0.0; - private double translationSpeedY = 0.0; - private double rotationSpeed = 0.0; - - private SwerveDrive() { - super(); - - for (int i = 0; i < MODULE_MOVEMENT_CAN_IDS.length; i++) { - modules[i] = new SwerveModule( - MODULE_MOVEMENT_CAN_IDS[i], - MODULE_ROTATION_CAN_IDS[i], - MODULE_CANCODER_CAN_IDS[i], - MODULE_CANCODER_OFFSETS[i], - MODULE_PHYSICAL_POSITIONS[i] - ); - } - - for (int i = 0; i < modules.length; i++) { - positions[i] = modules[i].getPosition(); - } - - kinematics = new SwerveDriveKinematics( - MODULE_PHYSICAL_POSITIONS[0], - MODULE_PHYSICAL_POSITIONS[1], - MODULE_PHYSICAL_POSITIONS[2], - MODULE_PHYSICAL_POSITIONS[3] - ); - - odometry = new SwerveDriveOdometry( - kinematics, - new Rotation2d(Pigeon.getYaw().radians()), - positions - ); - } - - /** - * Sets the current mode of the swerve drive. This changes the behavior of - * setMovementVector. - * @param mode The mode in which to operate. - */ - public static void setMode(SwerveMode mode) { - instance.mode = mode; - } - - /** - * Get the current swerve mode. - */ - public static SwerveMode getMode() { - return instance.mode; - } - - /** - * Gets the current display mode of the drive. The display mode is the - * either the current set mode or a temporary set mode but is not cleared - * at the end of a run loop, rather it is cleared once the next loop - * finishes, making it ideal for display purposes (e.i. LED lights). - */ - public static SwerveMode getDisplayMode() { - return instance.displayMode; - } - - /** - * May be called before running the swerve drive to temporarily set a mode - * for that call of a run method. - * - * Each time a run method completes it will change the mode back to what it - * was when this method was called. - * - * If this method is called more than once before running then the latest - * given temporary mode will be used, the inactive mode to be reset to after - * running will not be effected - */ - public static void tempMode(SwerveMode mode) { - if (instance.inactiveMode != null) { - instance.mode = mode; - return; - } - - instance.inactiveMode = instance.mode; - instance.mode = mode; - } - - /** - * Exactly like `tempMode` but predicates the temporary mode on the given - * boolean condition. - */ - public static void conditionalTempMode(SwerveMode mode, boolean condition) { - if (!condition) { - return; - } - - tempMode(mode); - } - - /** - * Gets the translation speed along the X axis. In headless mode this gives - * a speed along the field's x-axis, otheriwse it is the forward speed of - * the robot. - */ - public static double getTranslationSpeedX() { - return instance.translationSpeedX; - } - - /** - * Gets the translation speed along the Y axis. In headless mode this gives - * a speed along the field's y-axis, otheriwse it is the sideways speed of - * the robot. - */ - public static double getTranslationSpeedY() { - return instance.translationSpeedY; - } - - /** - * Gets the rotation speed of the robot. - */ - public static double getRotationSpeed() { - return instance.rotationSpeed; - } - - /** - * Sets the curve function for directional inputs (translations). - * @param curve The BiFunction to use for proccessing the curve, the first - * argument is what should be curved, the second is used for context. Return - * the curved direction. - */ - public static void setTranslationCurve(BiFunction curve) { - instance.translationCurve = curve; - } - - /** - * Gets the current curve used for directional inputs. - */ - public static BiFunction getTranslationCurve() { - return instance.translationCurve; - } - - /** - * Temporarily sets the curve function for directional inputs (translations). - * This action will atomatically be undone after calling a run method. - * @param curve The BiFunction to use for proccessing the curve, the first - * argument is what should be curved, the second is used for context. Return - * the curved direction. - */ - public static void tempTranslationCurve(BiFunction curve) { - if (instance.inactiveTransationCurve != null) { - instance.translationCurve = curve; - return; - } - - instance.inactiveTransationCurve = instance.translationCurve; - instance.translationCurve = curve; - } - - /** - * Exactly like `tempDirectionalCurve` but predicated on a boolean condition. - */ - public static void conditionalTempTranslationCurve( - BiFunction curve, - boolean condition - ) { - if (!condition) { - return; - } - - tempTranslationCurve(curve); - } - - /** - * Sets the curve function for turn inputs. - * @param curve The Function to use for proccessing the curve. - */ - public static void setRotationCurve(Function curve) { - instance.rotationCurve = curve; - } - - /** - * Gets the Function currently used for turning. - */ - public static Function getRotationCurve() { - return instance.rotationCurve; - } - - /** - * Temporarily sets the curve function for turn inputs. Undone after running. - * @param curve The Function to use for proccessing the curve. - */ - public static void tempRotationCurve(Function curve) { - if (instance.inactiveRotationCurve != null) { - instance.rotationCurve = curve; - return; - } - - instance.inactiveRotationCurve = instance.rotationCurve; - instance.rotationCurve = curve; - } - - /** - * Exactly like `tempTurnCurve` but predicated on a boolean condition. - */ - public static void conditionalTempRotationCurve( - Function curve, - boolean condition - ) { - if (!condition) { - return; - } - - tempRotationCurve(curve); - } - - /** - * Sets whether or not to limit the acceleration of the drive. - */ - public static void setAccelerationRate(double accelerationRate) { - for (SwerveModule module : instance.modules) { - module.setAccelerationRate(accelerationRate); - } - } - - /** - * Gets the acceleration rate limit of the drive. - */ - public static double getAccelerationRate() { - return instance.modules[0].getAccelerationRate(); - } - - /** - * Runs swerve, behavior changes based on the drive's mode. This will reset - * temporary modes on completion. - * @param translationX The X axis of the directional control, between 1 and -1 - * @param translationY The Y axis of the directional control, between 1 and -1. - * @param speed The speed scalar for the drive. - * @param rotation A value between 1 and -1 that determines the turning speed. - * @param lowSense The angle to move in low sensitivity in degrees, -1 for no movement. - */ - public static void run( - double translationX, - double translationY, - double speed, - double rotation - ) { - speed = Math.abs(translationX) <= 0.05 && Math.abs(translationY) <= 0.05 ? 0.0 : speed; - - // angle is in radians as per Java's trig methods. - var angle = Math.atan2(translationY, translationX); - translationX = Math.cos(angle) * speed; - - translationY = Math.sin(angle) * speed; - run(translationX, translationY, rotation); - } - - /** - * Runs swerve, behavior changes based on the drive's mode. Derives speed - * from directional inputs. This will reset temporary modes on completion. - * @param translationX The X axis of the directional control, between 1 and -1 - * @param translationY The Y axis of the directional control, between 1 and -1. - * @param rotation A value between 1 and -1 that determines the turning speed. - * @param lowSense The angle to move in low sensitivity in degrees, -1 for no movement. - */ - public static void run(double translationX, double translationY, double rotation) { - var x = instance.translationCurve.apply(translationX, translationY); - var y = instance.translationCurve.apply(translationY, translationX); - rotation = instance.rotationCurve.apply(rotation); - runUncurved(x, y, rotation); - } - - /** - * Run the swerve drive exactly by the arguments passed, no curving will - * occure on the given inputs, nor will they be deadbanded. This will reset - * temporary modes on completion. - * @param translationX Speed along the x axis. -1.0 - 1.0 - * @param translationY Speed along the y axis. -1.0 - 1.0 - * @param rotation Rate of turn. -1.0 - 1.0 - */ - public static void runUncurved(double translationX, double translationY, double rotation) { - instance.translationSpeedX = translationX; - instance.translationSpeedY = translationY; - instance.rotationSpeed = rotation; - - SwerveModuleState[] moduleStates = new SwerveModuleState[instance.modules.length]; - boolean holdPos = false; - - switch (instance.mode) { - case HEADLESS: - moduleStates = instance.kinematics.toSwerveModuleStates( - ChassisSpeeds.fromFieldRelativeSpeeds( - instance.translationSpeedX, - -instance.translationSpeedY, - instance.rotationSpeed, - new Rotation2d(Pigeon.getYaw().radians()) - ) - ); - break; - - case RELATIVE: - moduleStates = instance.kinematics.toSwerveModuleStates( - new ChassisSpeeds( - instance.translationSpeedX, - -instance.translationSpeedY, - instance.rotationSpeed - ) - ); - break; - - case ROCK: { - assert moduleStates.length == instance.modules.length; - holdPos = true; - - for (int i = 0; i < instance.modules.length; i++) { - moduleStates[i] = new SwerveModuleState( - 0.0, - new Rotation2d( - MODULE_ROCK_MODE_POSITIONS[i].radians() - ) - ); - } - - break; - } - - // This branch should never be reached as the enum used should never - // have more than the above possible values. - default: assert false; - } - - for (int i = 0; i < instance.modules.length; i++) { - instance.modules[i].setDesiredState(moduleStates[i]); - instance.modules[i].setRockMode(holdPos); - } - - instance.odometry.update(new Rotation2d(Pigeon.getYaw().radians()), instance.positions); - - // Reset temporary states - - instance.displayMode = instance.mode; - - if (instance.inactiveMode != null) { - instance.mode = instance.inactiveMode; - instance.inactiveMode = null; - } - - if (instance.inactiveTransationCurve != null) { - instance.translationCurve = instance.inactiveTransationCurve; - instance.inactiveTransationCurve = null; - } - - if (instance.inactiveRotationCurve != null) { - instance.rotationCurve = instance.inactiveRotationCurve; - instance.inactiveRotationCurve = null; - } - } - - /** - * Sets the max speed of all modules, use NaN for no limit. - */ - public static void setMaxSpeed(double maxSpeed) { - for (SwerveModule module : instance.modules) { - module.setMaxSpeed(maxSpeed); - } - } - - /** - * Gets the average tempurature of all motors on the drive in celsius. - */ - public static double getAverageMotorTemp() { - double tempSum = 0.0; - - for (SwerveModule module : instance.modules) { - tempSum += module.getRotationMotorTemp(); - tempSum += module.getMovementMotorTemp(); - } - - return tempSum / (instance.modules.length * 2.0); - } - - /** - * Gets the sum of all applied currents in amps of all motors on the drive. - */ - public static double getAppliedCurrent() { - double current = 0.0; - - for (SwerveModule module : instance.modules) { - current += module.getAppliedCurrent(); - } - - return current; - } - - /** - * Gets the average percent usage of each module's motor controller - * current pull. - */ - public static double getAveragePercentRatedCurrent() { - double percentSum = 0.0; - - for (SwerveModule module : instance.modules) { - percentSum += module.getPercentRatedCurrent(); - } - - return percentSum / (double)instance.modules.length; - } - - /** - * Print data to smart dashboard. - */ - @Override - public void print() { - SmartDashboard.putString("Swerve Drive Mode", getMode().toString()); - SmartDashboard.putString("Swerve Drive Odometry", - "(" + ((double)(long)(odometry.getPoseMeters().getX() * 100)) / 100 + ", " - + ((double)(long)(odometry.getPoseMeters().getY() * 100)) / 100 + ") " - + ((double)(long)(odometry.getPoseMeters().getRotation().getDegrees() * 100)) / 100 + " degrees" - ); - SmartDashboard.putNumber("Swerve Drive Average Motor Tempurature (Celsius)", getAverageMotorTemp()); - SmartDashboard.putNumber("Swerve Drive Total Current Pull (Amps)", getAppliedCurrent()); - SmartDashboard.putNumber("Swerve Drive Translation Speed X", translationSpeedX); - SmartDashboard.putNumber("Swerve Drive Translation Speed Y", translationSpeedY); - SmartDashboard.putNumber("Swerve Drive Rotation Speed", rotationSpeed); - } - - /** - * Gets the longest distance traveled by any modules. - */ - public static double getDistance() { - return Math.max(Math.abs(instance.modules[0].getDistanceTraveled()), Math.abs(instance.modules[1].getDistanceTraveled())); - } - - /** - * Zeros all movement encoder positions. - */ - public static void zeroPositions() { - for (SwerveModule module : instance.modules) { - module.zeroPositions(); - } - } -} +// 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 frc.robot.systems; + +import java.util.function.BiFunction; +import java.util.function.Function; + +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.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.SmartPrintable; +import frc.robot.subsystems.Angle; +import frc.robot.subsystems.SwerveMode; +import frc.robot.subsystems.SwerveModule; + +/* + * Manages the swerve drive train. + */ +public class SwerveDrive extends SmartPrintable { + private static final int MODULE_MOVEMENT_CAN_IDS[] = { 1, 2, 3, 4 }; + private static final int MODULE_ROTATION_CAN_IDS[] = { 5, 6, 7, 8 }; + private static final int MODULE_CANCODER_CAN_IDS[] = { 9, 10, 11, 12 }; + + private static final double CHASSIS_SIDE_LENGTH = 0.6; + + private static final Angle MODULE_CANCODER_OFFSETS[] = { + new Angle().setDegrees(-252.24607237 + 90.0), + new Angle().setDegrees(-224.033203125 + 270.0), + new Angle().setDegrees(-11.425719246268272 + 270.0), + new Angle().setDegrees(-179.56050113588573 + 90.0) + }; + + private static final Angle MODULE_ROCK_MODE_POSITIONS[] = { + new Angle().setRadians( -Angle.TAU / 8 ), + new Angle().setRadians( Angle.TAU / 8 ), + new Angle().setRadians( -Angle.TAU / 8 ), + new Angle().setRadians( Angle.TAU / 8 ) + }; + + private static final Translation2d MODULE_PHYSICAL_POSITIONS[] = { + new Translation2d( CHASSIS_SIDE_LENGTH / 2, CHASSIS_SIDE_LENGTH / 2 ), + new Translation2d( -CHASSIS_SIDE_LENGTH / 2, CHASSIS_SIDE_LENGTH / 2 ), + new Translation2d( -CHASSIS_SIDE_LENGTH / 2, -CHASSIS_SIDE_LENGTH / 2 ), + new Translation2d( CHASSIS_SIDE_LENGTH / 2, -CHASSIS_SIDE_LENGTH / 2 ) + }; + + // Singleton instance. + private static final SwerveDrive instance = new SwerveDrive(); + + // Internally mutable state objects + private final SwerveModule modules[] = new SwerveModule[MODULE_MOVEMENT_CAN_IDS.length]; + private final SwerveModulePosition positions[] = new SwerveModulePosition[MODULE_MOVEMENT_CAN_IDS.length]; + private final SwerveDriveKinematics kinematics; + private final SwerveDriveOdometry odometry; + + // Fully mutable state objects + private BiFunction translationCurve = Controls::defaultCurveTwoDimensional; + private BiFunction inactiveTransationCurve = null; + private Function rotationCurve = Controls::defaultCurve; + private Function inactiveRotationCurve = null; + private SwerveMode mode = SwerveMode.HEADLESS; + private SwerveMode inactiveMode = null; + private SwerveMode displayMode = SwerveMode.HEADLESS; + + private double translationSpeedX = 0.0; + private double translationSpeedY = 0.0; + private double rotationSpeed = 0.0; + + private SwerveDrive() { + super(); + + for (int i = 0; i < MODULE_MOVEMENT_CAN_IDS.length; i++) { + modules[i] = new SwerveModule( + MODULE_MOVEMENT_CAN_IDS[i], + MODULE_ROTATION_CAN_IDS[i], + MODULE_CANCODER_CAN_IDS[i], + MODULE_CANCODER_OFFSETS[i], + MODULE_PHYSICAL_POSITIONS[i] + ); + } + + for (int i = 0; i < modules.length; i++) { + positions[i] = modules[i].getPosition(); + } + + kinematics = new SwerveDriveKinematics( + MODULE_PHYSICAL_POSITIONS[0], + MODULE_PHYSICAL_POSITIONS[1], + MODULE_PHYSICAL_POSITIONS[2], + MODULE_PHYSICAL_POSITIONS[3] + ); + + odometry = new SwerveDriveOdometry( + kinematics, + new Rotation2d(Pigeon.getYaw().radians()), + positions + ); + } + + /** + * Sets the current mode of the swerve drive. This changes the behavior of + * setMovementVector. + * @param mode The mode in which to operate. + */ + public static void setMode(SwerveMode mode) { + instance.mode = mode; + } + + /** + * Get the current swerve mode. + */ + public static SwerveMode getMode() { + return instance.mode; + } + + /** + * Gets the current display mode of the drive. The display mode is the + * either the current set mode or a temporary set mode but is not cleared + * at the end of a run loop, rather it is cleared once the next loop + * finishes, making it ideal for display purposes (e.i. LED lights). + */ + public static SwerveMode getDisplayMode() { + return instance.displayMode; + } + + /** + * May be called before running the swerve drive to temporarily set a mode + * for that call of a run method. + * + * Each time a run method completes it will change the mode back to what it + * was when this method was called. + * + * If this method is called more than once before running then the latest + * given temporary mode will be used, the inactive mode to be reset to after + * running will not be effected + */ + public static void tempMode(SwerveMode mode) { + if (instance.inactiveMode != null) { + instance.mode = mode; + return; + } + + instance.inactiveMode = instance.mode; + instance.mode = mode; + } + + /** + * Exactly like `tempMode` but predicates the temporary mode on the given + * boolean condition. + */ + public static void conditionalTempMode(SwerveMode mode, boolean condition) { + if (!condition) { + return; + } + + tempMode(mode); + } + + /** + * Gets the translation speed along the X axis. In headless mode this gives + * a speed along the field's x-axis, otheriwse it is the forward speed of + * the robot. + */ + public static double getTranslationSpeedX() { + return instance.translationSpeedX; + } + + /** + * Gets the translation speed along the Y axis. In headless mode this gives + * a speed along the field's y-axis, otheriwse it is the sideways speed of + * the robot. + */ + public static double getTranslationSpeedY() { + return instance.translationSpeedY; + } + + /** + * Gets the rotation speed of the robot. + */ + public static double getRotationSpeed() { + return instance.rotationSpeed; + } + + /** + * Sets the curve function for directional inputs (translations). + * @param curve The BiFunction to use for proccessing the curve, the first + * argument is what should be curved, the second is used for context. Return + * the curved direction. + */ + public static void setTranslationCurve(BiFunction curve) { + instance.translationCurve = curve; + } + + /** + * Gets the current curve used for directional inputs. + */ + public static BiFunction getTranslationCurve() { + return instance.translationCurve; + } + + /** + * Temporarily sets the curve function for directional inputs (translations). + * This action will atomatically be undone after calling a run method. + * @param curve The BiFunction to use for proccessing the curve, the first + * argument is what should be curved, the second is used for context. Return + * the curved direction. + */ + public static void tempTranslationCurve(BiFunction curve) { + if (instance.inactiveTransationCurve != null) { + instance.translationCurve = curve; + return; + } + + instance.inactiveTransationCurve = instance.translationCurve; + instance.translationCurve = curve; + } + + /** + * Exactly like `tempDirectionalCurve` but predicated on a boolean condition. + */ + public static void conditionalTempTranslationCurve( + BiFunction curve, + boolean condition + ) { + if (!condition) { + return; + } + + tempTranslationCurve(curve); + } + + /** + * Sets the curve function for turn inputs. + * @param curve The Function to use for proccessing the curve. + */ + public static void setRotationCurve(Function curve) { + instance.rotationCurve = curve; + } + + /** + * Gets the Function currently used for turning. + */ + public static Function getRotationCurve() { + return instance.rotationCurve; + } + + /** + * Temporarily sets the curve function for turn inputs. Undone after running. + * @param curve The Function to use for proccessing the curve. + */ + public static void tempRotationCurve(Function curve) { + if (instance.inactiveRotationCurve != null) { + instance.rotationCurve = curve; + return; + } + + instance.inactiveRotationCurve = instance.rotationCurve; + instance.rotationCurve = curve; + } + + /** + * Exactly like `tempTurnCurve` but predicated on a boolean condition. + */ + public static void conditionalTempRotationCurve( + Function curve, + boolean condition + ) { + if (!condition) { + return; + } + + tempRotationCurve(curve); + } + + /** + * Sets whether or not to limit the acceleration of the drive. + */ + public static void setAccelerationRate(double accelerationRate) { + for (SwerveModule module : instance.modules) { + module.setAccelerationRate(accelerationRate); + } + } + + /** + * Gets the acceleration rate limit of the drive. + */ + public static double getAccelerationRate() { + return instance.modules[0].getAccelerationRate(); + } + + /** + * Runs swerve, behavior changes based on the drive's mode. This will reset + * temporary modes on completion. + * @param translationX The X axis of the directional control, between 1 and -1 + * @param translationY The Y axis of the directional control, between 1 and -1. + * @param speed The speed scalar for the drive. + * @param rotation A value between 1 and -1 that determines the turning speed. + * @param lowSense The angle to move in low sensitivity in degrees, -1 for no movement. + */ + public static void run( + double translationX, + double translationY, + double speed, + double rotation + ) { + speed = Math.abs(translationX) <= 0.05 && Math.abs(translationY) <= 0.05 ? 0.0 : speed; + + // angle is in radians as per Java's trig methods. + var angle = Math.atan2(translationY, translationX); + translationX = Math.cos(angle) * speed; + + translationY = Math.sin(angle) * speed; + run(translationX, translationY, rotation); + } + + /** + * Runs swerve, behavior changes based on the drive's mode. Derives speed + * from directional inputs. This will reset temporary modes on completion. + * @param translationX The X axis of the directional control, between 1 and -1 + * @param translationY The Y axis of the directional control, between 1 and -1. + * @param rotation A value between 1 and -1 that determines the turning speed. + * @param lowSense The angle to move in low sensitivity in degrees, -1 for no movement. + */ + public static void run(double translationX, double translationY, double rotation) { + var x = instance.translationCurve.apply(translationX, translationY); + var y = instance.translationCurve.apply(translationY, translationX); + rotation = instance.rotationCurve.apply(rotation); + runUncurved(x, y, rotation); + } + + /** + * Run the swerve drive exactly by the arguments passed, no curving will + * occure on the given inputs, nor will they be deadbanded. This will reset + * temporary modes on completion. + * @param translationX Speed along the x axis. -1.0 - 1.0 + * @param translationY Speed along the y axis. -1.0 - 1.0 + * @param rotation Rate of turn. -1.0 - 1.0 + */ + public static void runUncurved(double translationX, double translationY, double rotation) { + instance.translationSpeedX = translationX; + instance.translationSpeedY = translationY; + instance.rotationSpeed = rotation; + + SwerveModuleState[] moduleStates = new SwerveModuleState[instance.modules.length]; + boolean holdPos = false; + + switch (instance.mode) { + case HEADLESS: + moduleStates = instance.kinematics.toSwerveModuleStates( + ChassisSpeeds.fromFieldRelativeSpeeds( + instance.translationSpeedX, + -instance.translationSpeedY, + instance.rotationSpeed, + new Rotation2d(Pigeon.getYaw().radians()) + ) + ); + break; + + case RELATIVE: + moduleStates = instance.kinematics.toSwerveModuleStates( + new ChassisSpeeds( + instance.translationSpeedX, + -instance.translationSpeedY, + instance.rotationSpeed + ) + ); + break; + + case ROCK: { + assert moduleStates.length == instance.modules.length; + holdPos = true; + + for (int i = 0; i < instance.modules.length; i++) { + moduleStates[i] = new SwerveModuleState( + 0.0, + new Rotation2d( + MODULE_ROCK_MODE_POSITIONS[i].radians() + ) + ); + } + + break; + } + + // This branch should never be reached as the enum used should never + // have more than the above possible values. + default: assert false; + } + + for (int i = 0; i < instance.modules.length; i++) { + instance.modules[i].setDesiredState(moduleStates[i]); + instance.modules[i].setRockMode(holdPos); + } + + instance.odometry.update(new Rotation2d(Pigeon.getYaw().radians()), instance.positions); + + // Reset temporary states + + instance.displayMode = instance.mode; + + if (instance.inactiveMode != null) { + instance.mode = instance.inactiveMode; + instance.inactiveMode = null; + } + + if (instance.inactiveTransationCurve != null) { + instance.translationCurve = instance.inactiveTransationCurve; + instance.inactiveTransationCurve = null; + } + + if (instance.inactiveRotationCurve != null) { + instance.rotationCurve = instance.inactiveRotationCurve; + instance.inactiveRotationCurve = null; + } + } + + /** + * Sets the max speed of all modules, use NaN for no limit. + */ + public static void setMaxSpeed(double maxSpeed) { + for (SwerveModule module : instance.modules) { + module.setMaxSpeed(maxSpeed); + } + } + + /** + * Gets the average tempurature of all motors on the drive in celsius. + */ + public static double getAverageMotorTemp() { + double tempSum = 0.0; + + for (SwerveModule module : instance.modules) { + tempSum += module.getRotationMotorTemp(); + tempSum += module.getMovementMotorTemp(); + } + + return tempSum / (instance.modules.length * 2.0); + } + + /** + * Gets the sum of all applied currents in amps of all motors on the drive. + */ + public static double getAppliedCurrent() { + double current = 0.0; + + for (SwerveModule module : instance.modules) { + current += module.getAppliedCurrent(); + } + + return current; + } + + /** + * Gets the average percent usage of each module's motor controller + * current pull. + */ + public static double getAveragePercentRatedCurrent() { + double percentSum = 0.0; + + for (SwerveModule module : instance.modules) { + percentSum += module.getPercentRatedCurrent(); + } + + return percentSum / (double)instance.modules.length; + } + + /** + * Print data to smart dashboard. + */ + @Override + public void print() { + SmartDashboard.putString("Swerve Drive Mode", getMode().toString()); + SmartDashboard.putString("Swerve Drive Odometry", + "(" + ((double)(long)(odometry.getPoseMeters().getX() * 100)) / 100 + ", " + + ((double)(long)(odometry.getPoseMeters().getY() * 100)) / 100 + ") " + + ((double)(long)(odometry.getPoseMeters().getRotation().getDegrees() * 100)) / 100 + " degrees" + ); + SmartDashboard.putNumber("Swerve Drive Average Motor Tempurature (Celsius)", getAverageMotorTemp()); + SmartDashboard.putNumber("Swerve Drive Total Current Pull (Amps)", getAppliedCurrent()); + SmartDashboard.putNumber("Swerve Drive Translation Speed X", translationSpeedX); + SmartDashboard.putNumber("Swerve Drive Translation Speed Y", translationSpeedY); + SmartDashboard.putNumber("Swerve Drive Rotation Speed", rotationSpeed); + } + + /** + * Gets the longest distance traveled by any modules. + */ + public static double getDistance() { + return Math.max(Math.abs(instance.modules[0].getDistanceTraveled()), Math.abs(instance.modules[1].getDistanceTraveled())); + } + + /** + * Zeros all movement encoder positions. + */ + public static void zeroPositions() { + for (SwerveModule module : instance.modules) { + module.zeroPositions(); + } + } +} From eaa2e98b1aa6761a749645ef5aec2e27bad93089 Mon Sep 17 00:00:00 2001 From: JustBeChill Date: Thu, 12 Oct 2023 16:48:37 -0700 Subject: [PATCH 8/8] fix write lock not unlocking --- .../java/frc/robot/subsystems/WriteLock.java | 1 + .../frc/robot/systems/AutonomousRunner.java | 420 +++++++++--------- src/main/java/frc/robot/systems/Pigeon.java | 2 +- 3 files changed, 212 insertions(+), 211 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/WriteLock.java b/src/main/java/frc/robot/subsystems/WriteLock.java index 8fb299b..7eb1218 100644 --- a/src/main/java/frc/robot/subsystems/WriteLock.java +++ b/src/main/java/frc/robot/subsystems/WriteLock.java @@ -40,5 +40,6 @@ public void unlock() { */ public void unlock(T obj) { this.obj = obj; + locked = false; } } diff --git a/src/main/java/frc/robot/systems/AutonomousRunner.java b/src/main/java/frc/robot/systems/AutonomousRunner.java index de90a5a..2f59e67 100644 --- a/src/main/java/frc/robot/systems/AutonomousRunner.java +++ b/src/main/java/frc/robot/systems/AutonomousRunner.java @@ -1,211 +1,211 @@ -package frc.robot.systems; - -import java.security.InvalidParameterException; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.*; - -import frc.robot.systems.ElevFourbar.GamePiece; -import frc.robot.systems.Intake.SolenoidState; -import frc.robot.subsystems.DoubleSetpoint; -import frc.robot.subsystems.Setpoint; -import frc.robot.SmartPrintable; - -public class AutonomousRunner extends SmartPrintable { - private static final double OUTTAKE_SPEED_CUBE = 0.3; - private static final double OUTTAKE_SPEED_CONE = 0.4; - - private static final Runnable[] MODES = { - () -> {}, - AutonomousRunner::modeOne, - AutonomousRunner::modeTwo, - AutonomousRunner::modeThree, - AutonomousRunner::modeFour, - AutonomousRunner::modeFive, - AutonomousRunner::modeSix, - AutonomousRunner::modeSeven - }; - - private static final AutonomousRunner instance = new AutonomousRunner(); - - private Timer timer; - private Timer secondaryTimer; - - private int autoMode; - private int autoStage; - - private AutonomousRunner() { - super(); - - timer = new Timer(); - secondaryTimer = new Timer(); - - autoMode = 0; - autoStage = 0; - } - - /** - * Set the current autonomous mode, should probably be called only once in - * the autonomous initialization stage. - */ - public static void setAutoMode(int mode) { - if (mode < 0 || mode >= MODES.length) { - throw new InvalidParameterException("Auto mode must be between 0 and 7 (inclusive)"); - } - - instance.autoMode = mode; - } - - public static void init() { - instance.timer.reset(); - instance.timer.start(); - - instance.secondaryTimer.reset(); - instance.secondaryTimer.start(); - - instance.autoStage = 0; - } - - public static void run() { - SmartDashboard.putNumber("Auto Stage", instance.autoStage); - - //Run the auto mode - MODES[instance.autoMode].run(); - } - - /** - * Only move out of the community - */ - private static void modeOne() { - /******************* - * EXIT THE COMUNITY - * ***************** - */ - if (SwerveDrive.getDistance() < 670 && (instance.autoStage == 2 || instance.timer.get() > 10)) { - SwerveDrive.runUncurved(0.0, -0.6, 0.0); - } else { - SwerveDrive.runUncurved(0.0, 0.0, 0.0); - } - } - - /** - * Score only mid - */ - private static void modeTwo() { - score(ElevFourbar.MID_SCOORING_SETPOINT); - } - - /** - * Scores mid then moves out of community - */ - private static void modeThree() { - - score(ElevFourbar.MID_SCOORING_SETPOINT); - - /******************* - * EXIT THE COMUNITY - * ***************** - */ - if (SwerveDrive.getDistance() < 670 && (instance.autoStage == 2 || instance.timer.get() > 10)) { - SwerveDrive.runUncurved(0.0, -0.6, 0.0); - } else { - SwerveDrive.runUncurved(0.0, 0.0, 0.0); - } - } - - /** - * Score mid then auto balance - */ - private static void modeFour() { - - score(ElevFourbar.MID_SCOORING_SETPOINT); - - if (instance.autoStage > 1 || instance.timer.get() > 6) { - AutoBalance.run(); - } - } - - /** - * Only score high - */ - private static void modeFive() { - score(ElevFourbar.HIGH_SCORING_SETPOINT); - } - - /** - * Score high then move out of the community - */ - private static void modeSix() { - - score(ElevFourbar.HIGH_SCORING_SETPOINT); - - - /******************* - * EXIT THE COMUNITY - * ***************** - */ - if (SwerveDrive.getDistance() < 670 && (instance.autoStage == 2 || instance.timer.get() > 10)) { - SwerveDrive.runUncurved(0.0, -0.6, 0.0); - } else { - SwerveDrive.runUncurved(0.0, 0.0, 0.0); - } - } - - /** - * Score high and auto balance - */ - private static void modeSeven() { - score(ElevFourbar.HIGH_SCORING_SETPOINT); - - if (instance.autoStage > 1 || instance.timer.get() > 6) { - AutoBalance.run(); - } - } - - private static void score(DoubleSetpoint setpoint) { - if(instance.autoStage == 0){ - //Move the pivot up - Intake.autoPivot(SolenoidState.UP); - - ElevFourbar.setSetpointAuto((ElevFourbar.getGamePieceSelected() == GamePiece.CUBE ? setpoint.cube : setpoint.cone)); - ElevFourbar.autoRun(); - - //Move the elevator to the high scoring position - if(instance.timer.get() > 1.5) { - //Open claw when the position has been reached - Intake.autoClaw(SolenoidState.OPEN); - Intake.runRoller((ElevFourbar.getGamePieceSelected() == ElevFourbar.GamePiece.CUBE) ? OUTTAKE_SPEED_CUBE : OUTTAKE_SPEED_CONE); //run intake out - } - - if(instance.timer.get() > 2.5) { - instance.autoStage++; - } - } else if(instance.autoStage == 1) { - //1 second delay to prevent closing on the cube again >:( - if(instance.secondaryTimer.get() > 1) { - ElevFourbar.setSetpointAuto(new Setpoint(0, Setpoint.HYPOTENUSE)); - ElevFourbar.autoRun(); - - //Move to stowed setpoint - if(instance.secondaryTimer.get() > 2) { - //Close the claw and put the pivot down - instance.autoStage++; - } - - if(instance.secondaryTimer.get() > 2.5) { - Intake.autoPivot(SolenoidState.DOWN); - Intake.runRoller(0); //stop intake - } - } - } - } - - @Override - public void print() { - SmartDashboard.putNumber("Auto Timer 0", timer.get()); - SmartDashboard.putNumber("Auto Timer 1", secondaryTimer.get()); - SmartDashboard.putNumber("Auto Mode", autoMode); - SmartDashboard.putNumber("Auto Mode Stage", autoStage); - } -} +package frc.robot.systems; + +import java.security.InvalidParameterException; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.*; + +import frc.robot.systems.ElevFourbar.GamePiece; +import frc.robot.systems.Intake.SolenoidState; +import frc.robot.subsystems.DoubleSetpoint; +import frc.robot.subsystems.Setpoint; +import frc.robot.SmartPrintable; + +public class AutonomousRunner extends SmartPrintable { + private static final double OUTTAKE_SPEED_CUBE = 0.3; + private static final double OUTTAKE_SPEED_CONE = 0.4; + + private static final Runnable[] MODES = { + () -> {}, + AutonomousRunner::modeOne, + AutonomousRunner::modeTwo, + AutonomousRunner::modeThree, + AutonomousRunner::modeFour, + AutonomousRunner::modeFive, + AutonomousRunner::modeSix, + AutonomousRunner::modeSeven + }; + + private static final AutonomousRunner instance = new AutonomousRunner(); + + private Timer timer; + private Timer secondaryTimer; + + private int autoMode; + private int autoStage; + + private AutonomousRunner() { + super(); + + timer = new Timer(); + secondaryTimer = new Timer(); + + autoMode = 0; + autoStage = 0; + } + + /** + * Set the current autonomous mode, should probably be called only once in + * the autonomous initialization stage. + */ + public static void setAutoMode(int mode) { + if (mode < 0 || mode >= MODES.length) { + throw new InvalidParameterException("Auto mode must be between 0 and 7 (inclusive)"); + } + + instance.autoMode = mode; + } + + public static void init() { + instance.timer.reset(); + instance.timer.start(); + + instance.secondaryTimer.reset(); + instance.secondaryTimer.start(); + + instance.autoStage = 0; + } + + public static void run() { + SmartDashboard.putNumber("Auto Stage", instance.autoStage); + + //Run the auto mode + MODES[instance.autoMode].run(); + } + + /** + * Only move out of the community + */ + private static void modeOne() { + /******************* + * EXIT THE COMUNITY + * ***************** + */ + if (SwerveDrive.getDistance() < 670 && (instance.autoStage == 2 || instance.timer.get() > 10)) { + SwerveDrive.runUncurved(0.0, -0.6, 0.0); + } else { + SwerveDrive.runUncurved(0.0, 0.0, 0.0); + } + } + + /** + * Score only mid + */ + private static void modeTwo() { + score(ElevFourbar.MID_SCOORING_SETPOINT); + } + + /** + * Scores mid then moves out of community + */ + private static void modeThree() { + + score(ElevFourbar.MID_SCOORING_SETPOINT); + + /******************* + * EXIT THE COMUNITY + * ***************** + */ + if (SwerveDrive.getDistance() < 670 && (instance.autoStage == 2 || instance.timer.get() > 10)) { + SwerveDrive.runUncurved(0.0, -0.6, 0.0); + } else { + SwerveDrive.runUncurved(0.0, 0.0, 0.0); + } + } + + /** + * Score mid then auto balance + */ + private static void modeFour() { + + score(ElevFourbar.MID_SCOORING_SETPOINT); + + if (instance.autoStage > 1 || instance.timer.get() > 6) { + AutoBalance.run(); + } + } + + /** + * Only score high + */ + private static void modeFive() { + score(ElevFourbar.HIGH_SCORING_SETPOINT); + } + + /** + * Score high then move out of the community + */ + private static void modeSix() { + + score(ElevFourbar.HIGH_SCORING_SETPOINT); + + + /******************* + * EXIT THE COMUNITY + * ***************** + */ + if (SwerveDrive.getDistance() < 670 && (instance.autoStage == 2 || instance.timer.get() > 10)) { + SwerveDrive.runUncurved(0.0, -0.6, 0.0); + } else { + SwerveDrive.runUncurved(0.0, 0.0, 0.0); + } + } + + /** + * Score high and auto balance + */ + private static void modeSeven() { + score(ElevFourbar.HIGH_SCORING_SETPOINT); + + if (instance.autoStage > 1 || instance.timer.get() > 6) { + AutoBalance.run(); + } + } + + private static void score(DoubleSetpoint setpoint) { + if(instance.autoStage == 0){ + //Move the pivot up + Intake.autoPivot(SolenoidState.UP); + + ElevFourbar.setSetpointAuto((ElevFourbar.getGamePieceSelected() == GamePiece.CUBE ? setpoint.cube : setpoint.cone)); + ElevFourbar.autoRun(); + + //Move the elevator to the high scoring position + if(instance.timer.get() > 1.5) { + //Open claw when the position has been reached + Intake.autoClaw(SolenoidState.OPEN); + Intake.runRoller((ElevFourbar.getGamePieceSelected() == ElevFourbar.GamePiece.CUBE) ? OUTTAKE_SPEED_CUBE : OUTTAKE_SPEED_CONE); //run intake out + } + + if(instance.timer.get() > 2.5) { + instance.autoStage++; + } + } else if(instance.autoStage == 1) { + //1 second delay to prevent closing on the cube again >:( + if(instance.secondaryTimer.get() > 1) { + ElevFourbar.setSetpointAuto(new Setpoint(0, Setpoint.HYPOTENUSE)); + ElevFourbar.autoRun(); + + //Move to stowed setpoint + if(instance.secondaryTimer.get() > 2) { + //Close the claw and put the pivot down + instance.autoStage++; + } + + if(instance.secondaryTimer.get() > 2.5) { + Intake.autoPivot(SolenoidState.DOWN); + Intake.runRoller(0); //stop intake + } + } + } + } + + @Override + public void print() { + SmartDashboard.putNumber("Auto Timer 0", timer.get()); + SmartDashboard.putNumber("Auto Timer 1", secondaryTimer.get()); + SmartDashboard.putNumber("Auto Mode", autoMode); + SmartDashboard.putNumber("Auto Mode Stage", autoStage); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/systems/Pigeon.java b/src/main/java/frc/robot/systems/Pigeon.java index 35a689c..ec0b3ab 100644 --- a/src/main/java/frc/robot/systems/Pigeon.java +++ b/src/main/java/frc/robot/systems/Pigeon.java @@ -34,7 +34,7 @@ private Pigeon(int canID) { super(); pigeon = new Pigeon2(canID); - changePerSecond = new WriteLock(new OrientationalChange( + changePerSecond = new WriteLock<>(new OrientationalChange( new Angle().setDegrees(0.0), new Angle().setDegrees(0.0), new Angle().setDegrees(0.0)