From 615aa614baa97e544cecf359a4309299bba68fe3 Mon Sep 17 00:00:00 2001 From: Evan Overman Date: Sat, 27 May 2023 13:34:04 -0700 Subject: [PATCH 01/11] starting of, module is mostly radians now --- .../frc/robot/subsystems/SwerveModule.java | 100 +++++++++++------- .../java/frc/robot/systems/SwerveDrive.java | 10 +- 2 files changed, 67 insertions(+), 43 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index a887fc4..81e0134 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -6,7 +6,7 @@ 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; @@ -17,6 +17,7 @@ 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; /** @@ -28,13 +29,13 @@ public class SwerveModule extends SmartPrintable { private static final double CONVERSION_FACTOR_ROTATION_VELOCITY = CONVERSION_FACTOR_ROTATION * (1 / 60); // RPM to degrees per second. private static final double CONVERSION_FACTOR_MOVEMENT_VELOCITY = CONVERSION_FACTOR_MOVEMENT * (1 / 60); // RPM to meters per second. - private static double PID_P = 0.01; - private static double PID_I = 0.0; - private static double PID_D = 0.0; + private static final double PID_P = 0.01; + private static final double PID_I = 0.0; + private static final double PID_D = 0.0; - private static double ROCK_PID_P = 0.1; - private static double ROCK_PID_I = 0.0; - private static double ROCK_PID_D = 0.0; + private static final double ROCK_PID_P = 0.1; + private static final double ROCK_PID_I = 0.0; + private static final double ROCK_PID_D = 0.0; private final SwerveModulePosition position; @@ -51,22 +52,20 @@ public class SwerveModule extends SmartPrintable { private final double canCoderOffset; private final double coefficient; - /** - * 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. - */ - private double rockPos; + // 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; public SwerveModule(int movementMotorID, int rotationalMotorID, int canCoderID, double canCoderOffset, double coefficient) { super(); - this.canCoderOffset = canCoderOffset; - this.coefficient = coefficient; + this.canCoderOffset = canCoderOffset / 180.0 * Math.PI; + this.coefficient = coefficient / 180.0 * Math.PI; rotationMotor = new CANSparkMax(rotationalMotorID, MotorType.kBrushless); rotationMotor.setInverted(false); - //rotationMotor.setVoltage(11.0); rotationMotor.setSmartCurrentLimit(30); movementMotor = new CANSparkMax(movementMotorID, MotorType.kBrushless); @@ -75,9 +74,21 @@ public SwerveModule(int movementMotorID, int rotationalMotorID, int canCoderID, movementMotor.setSmartCurrentLimit(40); angularEncoder = new CANCoder(canCoderID); - angularEncoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition, 1000); + + // 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. + angularEncoder.configGetFeedbackCoefficient() / 180.0 * Math.PI, + "rad", + SensorTimeBase.PerSecond + ); rotationEncoder = rotationMotor.getEncoder(); + rotationEncoder.setPositionConversionFactor(180.0 / Math.PI); rotationEncoder.setPosition(angularEncoder.getPosition()); rotationEncoder.setPositionConversionFactor(CONVERSION_FACTOR_ROTATION); rotationEncoder.setVelocityConversionFactor(CONVERSION_FACTOR_ROTATION_VELOCITY); @@ -88,33 +99,29 @@ public SwerveModule(int movementMotorID, int rotationalMotorID, int canCoderID, movementEncoder.setVelocityConversionFactor(CONVERSION_FACTOR_MOVEMENT_VELOCITY); rotationController = new PIDController(PID_P, PID_I, PID_D); - rotationController.enableContinuousInput(0.0, 360.0); - rotationController.setTolerance(0.5); + rotationController.enableContinuousInput(0.0, Math.TAU); + rotationController.setTolerance(0.005); // This is mroe precise than before, TODO: test. rockController = new PIDController(ROCK_PID_P, ROCK_PID_I, ROCK_PID_D); rockController.setTolerance(0.5); - position = new SwerveModulePosition(movementEncoder.getPosition() * CONVERSION_FACTOR_MOVEMENT, new Rotation2d(angularEncoder.getPosition() / 180.0 * Math.PI)); + position = new SwerveModulePosition(movementEncoder.getPosition() * CONVERSION_FACTOR_MOVEMENT, new Rotation2d(angularEncoder.getPosition())); } /** * Sets the desired module state for this module. */ public void setDesiredState(SwerveModuleState state) { - /* if (Math.abs(state.speedMetersPerSecond) < 0.001) { - movementMotor.set(0.0); - rotationMotor.set(0.0); - return; - } */ - - double currentPosition = (angularEncoder.getPosition() + canCoderOffset) % 360.0; - state = SwerveModuleState.optimize(state, new Rotation2d(currentPosition * Math.PI / 180.0)); - double calculation = rotationController.calculate(currentPosition, (state.angle.getDegrees() + 360.0) % 360.0); + double currentPosition = (angularEncoder.getPosition() + canCoderOffset) % Math.TAU; + state = SwerveModuleState.optimize(state, new Rotation2d(currentPosition)); + double calculation = rotationController.calculate(currentPosition, (state.angle.getRadians() + Math.TAU) % Math.TAU); if (rockPos != rockPos) { - movementMotor.set(Math.abs(state.speedMetersPerSecond) > 0.3 ? - Math.signum(state.speedMetersPerSecond) * 0.3 : - state.speedMetersPerSecond); + movementMotor.set( + Math.abs(state.speedMetersPerSecond) > maxSpeed + ? Math.signum(state.speedMetersPerSecond) * maxSpeed + : state.speedMetersPerSecond + ); } rotationMotor.set(calculation * coefficient); @@ -133,42 +140,57 @@ public void setRockMode(boolean shouldHold) { } if (shouldHold && rockPos != rockPos) { - rockPos = getPosition(); + rockPos = getDistanceTraveled(); } - movementMotor.set(rockController.calculate(getPosition(), rockPos)); + // TODO: move the running of rock mode elsewhere... + movementMotor.set(rockController.calculate(getDistanceTraveled(), rockPos)); } /** * True if in rock mode. */ - public boolean getRockMode() { + public boolean inRockMode() { return rockPos == rockPos; } + /** + * Sets this module's maximum movement speed + */ + public void setMaxSpeed(double maxSpeed) { + this.maxSpeed = maxSpeed; + } + + /** + * Gets this module's maximum movement speed + */ + public double getMaxSpeed() { + return maxSpeed; + } + /** * 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 getPosition() { + public double getDistanceTraveled() { return movementEncoder.getPosition(); } @Override public void print() { SmartDashboard.putNumber("Module " + angularEncoder.getDeviceID() + " Position", angularEncoder.getPosition()); - SmartDashboard.putNumber("Module " + angularEncoder.getDeviceID() + " Position mod 360", angularEncoder.getPosition() % 360); + SmartDashboard.putNumber("Module " + angularEncoder.getDeviceID() + " Position mod tau", angularEncoder.getPosition() % Math.TAU); SmartDashboard.putNumber("Module " + angularEncoder.getDeviceID() + " Position + off", angularEncoder.getPosition() + canCoderOffset); - SmartDashboard.putNumber("Module " + angularEncoder.getDeviceID() + " Position + off mod 360", (angularEncoder.getPosition() + canCoderOffset) % 360); + SmartDashboard.putNumber("Module " + angularEncoder.getDeviceID() + " Position + off mod tau", (angularEncoder.getPosition() + canCoderOffset) % Math.TAU); SmartDashboard.putNumber("Module " + angularEncoder.getDeviceID() + " Position (Distance) ", movementEncoder.getPosition()); } /** * Gets the angle of the module. */ - public SwerveModulePosition getAngle() { + public SwerveModulePosition getPosition() { return position; } diff --git a/src/main/java/frc/robot/systems/SwerveDrive.java b/src/main/java/frc/robot/systems/SwerveDrive.java index 5676f9e..d2b9294 100644 --- a/src/main/java/frc/robot/systems/SwerveDrive.java +++ b/src/main/java/frc/robot/systems/SwerveDrive.java @@ -31,7 +31,6 @@ public class SwerveDrive extends SmartPrintable { private static final int MODULE_CANCODER_CAN_IDS[] = { 9, 10, 11, 12 }; private static final double MODULE_CANCODER_OFFSETS[] = { -252.24607237 + 90.0, -224.033203125 + 270.0, -11.425719246268272 + 270.0, -179.56050113588573 + 90.0 }; - //private static final double MODULE_CANCODER_OFFSETS[] = { -252.24607237 + 90.0, -405.26363752 + 90.0, -189.66795267 + 90.0, -175.16600078 + 90.0 }; private static final double MODULE_ROCK_MODE_PSOITIONS[] = { -Math.PI / 4, Math.PI / 4, -Math.PI / 4, Math.PI / 4 }; private static final double MODULE_COEFFIENTS[] = { -1.0, -1.0, -1.0, -1.0 }; @@ -40,16 +39,19 @@ public class SwerveDrive extends SmartPrintable { private static final double CHASSIS_SIDE_LENGTH = 0.6; private static final double RADIAN_DEGREE_RATIO = Math.PI / 180.0; + // Singleton instance. private static final SwerveDrive instance = new SwerveDrive(); - private BiFunction directionCurve = Controls::defaultCurveTwoDimensional; - private Function turnCurve = Controls::defaultCurve; - + // These drive state objects modify themselves internally through methods, + // but require no setting and are therefore final. 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; + // Variable drive states objects. + private BiFunction directionCurve = Controls::defaultCurveTwoDimensional; + private Function turnCurve = Controls::defaultCurve; private SwerveMode mode = SwerveMode.Headless; private SwerveDrive() { From 3994193676f10ffef63691b77727d6a034653e00 Mon Sep 17 00:00:00 2001 From: Evan Overman Date: Sat, 27 May 2023 21:01:04 -0700 Subject: [PATCH 02/11] remove newline --- src/main/java/frc/robot/systems/Pigeon.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/systems/Pigeon.java b/src/main/java/frc/robot/systems/Pigeon.java index 9f3121d..e1e9d10 100644 --- a/src/main/java/frc/robot/systems/Pigeon.java +++ b/src/main/java/frc/robot/systems/Pigeon.java @@ -48,7 +48,6 @@ private Pigeon(int canID) { */ public static void setFeildZero() { instance.pigeon.setYaw(0.0); - } /** From 993242dae5cd4b1f1c506941ec3be938ca67d975 Mon Sep 17 00:00:00 2001 From: Evan Overman Date: Sat, 27 May 2023 21:01:18 -0700 Subject: [PATCH 03/11] fix a couple mistakes --- .../java/frc/robot/subsystems/SwerveModule.java | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 81e0134..5c941b9 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -24,9 +24,9 @@ * Class for managing and manipulating a swerve module. */ public class SwerveModule extends SmartPrintable { - private static final double CONVERSION_FACTOR_ROTATION = 150 / 7; // Rotations to degrees. + private static final double CONVERSION_FACTOR_ROTATION = (150 * Math.PI) / (7 * 180); // 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 degrees per second. + 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 PID_P = 0.01; @@ -88,7 +88,6 @@ public SwerveModule(int movementMotorID, int rotationalMotorID, int canCoderID, ); rotationEncoder = rotationMotor.getEncoder(); - rotationEncoder.setPositionConversionFactor(180.0 / Math.PI); rotationEncoder.setPosition(angularEncoder.getPosition()); rotationEncoder.setPositionConversionFactor(CONVERSION_FACTOR_ROTATION); rotationEncoder.setVelocityConversionFactor(CONVERSION_FACTOR_ROTATION_VELOCITY); @@ -100,12 +99,15 @@ public SwerveModule(int movementMotorID, int rotationalMotorID, int canCoderID, rotationController = new PIDController(PID_P, PID_I, PID_D); rotationController.enableContinuousInput(0.0, Math.TAU); - rotationController.setTolerance(0.005); // This is mroe precise than before, TODO: test. + rotationController.setTolerance(0.005); // This is more precise than before, TODO: test. rockController = new PIDController(ROCK_PID_P, ROCK_PID_I, ROCK_PID_D); rockController.setTolerance(0.5); - position = new SwerveModulePosition(movementEncoder.getPosition() * CONVERSION_FACTOR_MOVEMENT, new Rotation2d(angularEncoder.getPosition())); + position = new SwerveModulePosition( + movementEncoder.getPosition() * CONVERSION_FACTOR_MOVEMENT, + new Rotation2d(angularEncoder.getPosition()) + ); } /** @@ -114,7 +116,6 @@ public SwerveModule(int movementMotorID, int rotationalMotorID, int canCoderID, public void setDesiredState(SwerveModuleState state) { double currentPosition = (angularEncoder.getPosition() + canCoderOffset) % Math.TAU; state = SwerveModuleState.optimize(state, new Rotation2d(currentPosition)); - double calculation = rotationController.calculate(currentPosition, (state.angle.getRadians() + Math.TAU) % Math.TAU); if (rockPos != rockPos) { movementMotor.set( @@ -124,6 +125,7 @@ public void setDesiredState(SwerveModuleState state) { ); } + double calculation = rotationController.calculate(currentPosition, (state.angle.getRadians() + Math.TAU) % Math.TAU); rotationMotor.set(calculation * coefficient); position.angle = new Rotation2d(angularEncoder.getPosition()); From 735192377dedd55de90c22d1659c8c1ba8475a06 Mon Sep 17 00:00:00 2001 From: Evan Overman Date: Sat, 27 May 2023 21:01:29 -0700 Subject: [PATCH 04/11] clean up `SwerveDrive` --- .../java/frc/robot/systems/SwerveDrive.java | 54 ++++++++++--------- 1 file changed, 30 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/systems/SwerveDrive.java b/src/main/java/frc/robot/systems/SwerveDrive.java index d2b9294..11d90a6 100644 --- a/src/main/java/frc/robot/systems/SwerveDrive.java +++ b/src/main/java/frc/robot/systems/SwerveDrive.java @@ -30,14 +30,18 @@ public class SwerveDrive extends SmartPrintable { 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 MODULE_CANCODER_OFFSETS[] = { -252.24607237 + 90.0, -224.033203125 + 270.0, -11.425719246268272 + 270.0, -179.56050113588573 + 90.0 }; + // TODO: Get radian values from start dashboard. + private static final double MODULE_CANCODER_OFFSETS[] = { + Math.toRadians(-252.24607237 + 90.0), + Math.toRadians(-224.033203125 + 270.0), + Math.toRadians(-11.425719246268272 + 270.0), + Math.toRadians(-179.56050113588573 + 90.0) + }; + private static final double MODULE_ROCK_MODE_PSOITIONS[] = { -Math.PI / 4, Math.PI / 4, -Math.PI / 4, Math.PI / 4 }; private static final double MODULE_COEFFIENTS[] = { -1.0, -1.0, -1.0, -1.0 }; - private static final double LOW_SENSITIVITY_RATIO = 0.08; - private static final double CHASSIS_SIDE_LENGTH = 0.6; - private static final double RADIAN_DEGREE_RATIO = Math.PI / 180.0; // Singleton instance. private static final SwerveDrive instance = new SwerveDrive(); @@ -68,7 +72,7 @@ private SwerveDrive() { } for (int i = 0; i < modules.length; i++) { - positions[i] = modules[i].getAngle(); + positions[i] = modules[i].getPosition(); } kinematics = new SwerveDriveKinematics( @@ -78,7 +82,11 @@ private SwerveDrive() { new Translation2d( CHASSIS_SIDE_LENGTH / 2, -CHASSIS_SIDE_LENGTH / 2) ); - odometry = new SwerveDriveOdometry(kinematics, new Rotation2d(Pigeon.getYaw() * RADIAN_DEGREE_RATIO), positions); + odometry = new SwerveDriveOdometry( + kinematics, + new Rotation2d(Math.toRadians(Pigeon.getYaw())), + positions + ); } /** @@ -111,10 +119,10 @@ public static void run(double directionalX, double directionalY, double turn, in return; } - instance.odometry.update(new Rotation2d(Pigeon.getYaw() * RADIAN_DEGREE_RATIO), instance.positions); + instance.odometry.update(new Rotation2d(Math.toRadians(Pigeon.getYaw())), instance.positions); if (lowSense != -1) { - double angle = (2 * Math.PI) - ((double)lowSense * RADIAN_DEGREE_RATIO); + double angle = Math.TAU - (Math.toRadians((double)lowSense)); // inverted since the drive is rotated to compensate for joystick stuff directionalX = -(Math.sin(angle) * LOW_SENSITIVITY_RATIO); @@ -128,13 +136,12 @@ public static void run(double directionalX, double directionalY, double turn, in directionalY = instance.directionCurve.apply(directionalY, directionalX); turn = instance.turnCurve.apply(turn); - ChassisSpeeds chassisSpeeds; - - if (instance.mode == SwerveMode.Headless) { - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(directionalX, -directionalY, turn, new Rotation2d(Pigeon.getYaw() * RADIAN_DEGREE_RATIO)); - } else { - chassisSpeeds = new ChassisSpeeds(directionalX, -directionalY, turn); - } + ChassisSpeeds chassisSpeeds = instance.mode == SwerveMode.Headless + ? ChassisSpeeds.fromFieldRelativeSpeeds( + directionalX, -directionalY, turn, + new Rotation2d(Math.toRadians(Pigeon.getYaw())) + ) + : new ChassisSpeeds(directionalX, -directionalY, turn); SwerveModuleState[] moduleStates = instance.kinematics.toSwerveModuleStates(chassisSpeeds); @@ -156,13 +163,12 @@ public static void runUncurved(double directionalX, double directionalY, double return; } - ChassisSpeeds chassisSpeeds; - - if (instance.mode == SwerveMode.Headless) { - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(directionalX, -directionalY, turn, new Rotation2d(Pigeon.getYaw() * RADIAN_DEGREE_RATIO)); - } else { - chassisSpeeds = new ChassisSpeeds(directionalX, -directionalY, turn); - } + ChassisSpeeds chassisSpeeds = instance.mode == SwerveMode.Headless + ? ChassisSpeeds.fromFieldRelativeSpeeds( + directionalX, -directionalY, turn, + new Rotation2d(Math.toRadians(Pigeon.getYaw())) + ) + : new ChassisSpeeds(directionalX, -directionalY, turn); SwerveModuleState[] moduleStates = instance.kinematics.toSwerveModuleStates(chassisSpeeds); @@ -196,7 +202,7 @@ public static boolean getRockMode() { boolean rockMode = false; for (SwerveModule module : instance.modules) { - rockMode |= module.getRockMode(); + rockMode |= module.inRockMode(); } return rockMode; @@ -292,7 +298,7 @@ public void print() { * Gets the longest distance traveled by any modules. */ public static double getDistance() { - return Math.max(Math.abs(instance.modules[0].getPosition()), Math.abs(instance.modules[1].getPosition())); + return Math.max(Math.abs(instance.modules[0].getDistanceTraveled()), Math.abs(instance.modules[1].getDistanceTraveled())); } /** From 7e77786a32556da354ab8b9584d93972a3b14f0c Mon Sep 17 00:00:00 2001 From: Evan Overman Date: Sun, 28 May 2023 12:47:34 -0700 Subject: [PATCH 05/11] add and use angle class --- src/main/java/frc/robot/subsystems/Angle.java | 37 +++++++++++++++++++ .../frc/robot/subsystems/SwerveModule.java | 18 ++++----- .../java/frc/robot/systems/AutoBalance.java | 5 +-- src/main/java/frc/robot/systems/Pigeon.java | 20 +++++----- .../java/frc/robot/systems/SwerveDrive.java | 32 +++++++++------- 5 files changed, 78 insertions(+), 34 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/Angle.java diff --git a/src/main/java/frc/robot/subsystems/Angle.java b/src/main/java/frc/robot/subsystems/Angle.java new file mode 100644 index 0000000..8bd03ca --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Angle.java @@ -0,0 +1,37 @@ +// 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 { + private double dec = 0.0; + + public double radians() { + return dec * Math.TAU; + } + + public double degrees() { + return dec * 360.0; + } + + public Angle setRadians(double radians) { + dec = radians / Math.TAU; + return this; + } + + public Angle setDegrees(double degrees) { + dec = degrees / 360.0; + return this; + } + + /** + * Creates a new identical angle so that modification of the original will not + * effect the value returned from this function. + */ + public Angle clone() { + var ang = new Angle(); + ang.dec = this.dec; + return ang; + } +} diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 5c941b9..b68c5f1 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -24,7 +24,7 @@ * Class for managing and manipulating a swerve module. */ public class SwerveModule extends SmartPrintable { - private static final double CONVERSION_FACTOR_ROTATION = (150 * Math.PI) / (7 * 180); // Rotations to radians. + 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. @@ -49,7 +49,7 @@ public class SwerveModule extends SmartPrintable { private final PIDController rotationController; private final PIDController rockController; - private final double canCoderOffset; + private final Angle canCoderOffset; private final double coefficient; // Set to NaN if not in rock mode, NaN does not equal itself by definition @@ -58,11 +58,11 @@ public class SwerveModule extends SmartPrintable { private double rockPos = Double.NaN; private double maxSpeed = Double.NaN; - public SwerveModule(int movementMotorID, int rotationalMotorID, int canCoderID, double canCoderOffset, double coefficient) { + public SwerveModule(int movementMotorID, int rotationalMotorID, int canCoderID, Angle canCoderOffset, double coefficient) { super(); - this.canCoderOffset = canCoderOffset / 180.0 * Math.PI; - this.coefficient = coefficient / 180.0 * Math.PI; + this.canCoderOffset = canCoderOffset.clone(); + this.coefficient = coefficient; rotationMotor = new CANSparkMax(rotationalMotorID, MotorType.kBrushless); rotationMotor.setInverted(false); @@ -82,7 +82,7 @@ public SwerveModule(int movementMotorID, int rotationalMotorID, int canCoderID, // Since the default coefficiant used for degrees is not // particularly intuitive we just grab it and run a deg -> rad // conversion on it. - angularEncoder.configGetFeedbackCoefficient() / 180.0 * Math.PI, + Math.toRadians(angularEncoder.configGetFeedbackCoefficient()), "rad", SensorTimeBase.PerSecond ); @@ -114,7 +114,7 @@ public SwerveModule(int movementMotorID, int rotationalMotorID, int canCoderID, * Sets the desired module state for this module. */ public void setDesiredState(SwerveModuleState state) { - double currentPosition = (angularEncoder.getPosition() + canCoderOffset) % Math.TAU; + double currentPosition = (angularEncoder.getPosition() + canCoderOffset.radians()) % Math.TAU; state = SwerveModuleState.optimize(state, new Rotation2d(currentPosition)); if (rockPos != rockPos) { @@ -184,8 +184,8 @@ public double getDistanceTraveled() { public void print() { SmartDashboard.putNumber("Module " + angularEncoder.getDeviceID() + " Position", angularEncoder.getPosition()); SmartDashboard.putNumber("Module " + angularEncoder.getDeviceID() + " Position mod tau", angularEncoder.getPosition() % Math.TAU); - SmartDashboard.putNumber("Module " + angularEncoder.getDeviceID() + " Position + off", angularEncoder.getPosition() + canCoderOffset); - SmartDashboard.putNumber("Module " + angularEncoder.getDeviceID() + " Position + off mod tau", (angularEncoder.getPosition() + canCoderOffset) % Math.TAU); + SmartDashboard.putNumber("Module " + angularEncoder.getDeviceID() + " Position + off", angularEncoder.getPosition() + canCoderOffset.radians()); + SmartDashboard.putNumber("Module " + angularEncoder.getDeviceID() + " Position + off mod tau", (angularEncoder.getPosition() + canCoderOffset.radians()) % Math.TAU); SmartDashboard.putNumber("Module " + angularEncoder.getDeviceID() + " Position (Distance) ", movementEncoder.getPosition()); } diff --git a/src/main/java/frc/robot/systems/AutoBalance.java b/src/main/java/frc/robot/systems/AutoBalance.java index 4dd589b..5a707b2 100644 --- a/src/main/java/frc/robot/systems/AutoBalance.java +++ b/src/main/java/frc/robot/systems/AutoBalance.java @@ -33,7 +33,6 @@ public enum Stage { /** * auto balance is ramming, meant to dock the charge station. */ - RAMMING, /** @@ -141,7 +140,7 @@ private Stage adjust() { return Stage.PAUSED; } - SwerveDrive.runUncurved(0.0, Math.signum(Pigeon.getPitch()) * HALTING_SPEED, 0.0); + SwerveDrive.runUncurved(0.0, Math.signum(Pigeon.getPitch().degrees()) * HALTING_SPEED, 0.0); return Stage.ADJUSTING; } @@ -160,7 +159,7 @@ private Stage pause() { * Returns true if auto balance beleives it is balanced, not 100% accurate. */ private boolean level() { - return Math.abs(Pigeon.getPitch()) < 10.0 && Math.abs(Pigeon.getChangePerSecond().pitchPerSecond) < 10.0; + return Math.abs(Pigeon.getPitch().degrees()) < 10.0 && Math.abs(Pigeon.getChangePerSecond().pitchPerSecond) < 10.0; } /** diff --git a/src/main/java/frc/robot/systems/Pigeon.java b/src/main/java/frc/robot/systems/Pigeon.java index e1e9d10..2b09080 100644 --- a/src/main/java/frc/robot/systems/Pigeon.java +++ b/src/main/java/frc/robot/systems/Pigeon.java @@ -13,7 +13,9 @@ import com.ctre.phoenix.sensors.Pigeon2; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + import frc.robot.SmartPrintable; +import frc.robot.subsystems.Angle; /* * Manages the robot's pigeon. @@ -60,31 +62,31 @@ public static OrientationalChange getChangePerSecond() { /** * Gets Yaw. */ - public static double getYaw() { - return instance.pigeon.getYaw() % 360.0; + public static Angle getYaw() { + return new Angle().setDegrees(instance.pigeon.getYaw() % 360.0); } /** * Gets pitch. */ - public static double getPitch() { + public static Angle getPitch() { // Pitch and roll are flipped due to mounting orientation. - return instance.pigeon.getRoll(); + return new Angle().setDegrees(instance.pigeon.getRoll()); } /** * Gets roll. */ - public static double getRoll() { + public static Angle getRoll() { // Pitch and roll are flipped due to mounting orientation. - return instance.pigeon.getPitch(); + return new Angle().setDegrees(instance.pigeon.getPitch()); } @Override public void print() { - SmartDashboard.putNumber("Pigeon Yaw", getYaw()); - SmartDashboard.putNumber("Pigeon Pitch", getPitch()); - SmartDashboard.putNumber("Pigeon Roll", getRoll()); + SmartDashboard.putNumber("Pigeon Yaw", getYaw().radians()); + SmartDashboard.putNumber("Pigeon Pitch", getPitch().radians()); + SmartDashboard.putNumber("Pigeon Roll", getRoll().radians()); OrientationalChange change = getChangePerSecond(); diff --git a/src/main/java/frc/robot/systems/SwerveDrive.java b/src/main/java/frc/robot/systems/SwerveDrive.java index 11d90a6..600509e 100644 --- a/src/main/java/frc/robot/systems/SwerveDrive.java +++ b/src/main/java/frc/robot/systems/SwerveDrive.java @@ -19,6 +19,7 @@ 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; @@ -30,15 +31,20 @@ public class SwerveDrive extends SmartPrintable { private static final int MODULE_ROTATION_CAN_IDS[] = { 5, 6, 7, 8 }; private static final int MODULE_CANCODER_CAN_IDS[] = { 9, 10, 11, 12 }; - // TODO: Get radian values from start dashboard. - private static final double MODULE_CANCODER_OFFSETS[] = { - Math.toRadians(-252.24607237 + 90.0), - Math.toRadians(-224.033203125 + 270.0), - Math.toRadians(-11.425719246268272 + 270.0), - Math.toRadians(-179.56050113588573 + 90.0) + 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( -Math.PI / 4), + new Angle().setRadians( Math.PI / 4), + new Angle().setRadians( -Math.PI / 4), + new Angle().setRadians( Math.PI / 4) }; - private static final double MODULE_ROCK_MODE_PSOITIONS[] = { -Math.PI / 4, Math.PI / 4, -Math.PI / 4, Math.PI / 4 }; private static final double MODULE_COEFFIENTS[] = { -1.0, -1.0, -1.0, -1.0 }; private static final double LOW_SENSITIVITY_RATIO = 0.08; private static final double CHASSIS_SIDE_LENGTH = 0.6; @@ -84,7 +90,7 @@ private SwerveDrive() { odometry = new SwerveDriveOdometry( kinematics, - new Rotation2d(Math.toRadians(Pigeon.getYaw())), + new Rotation2d(Pigeon.getYaw().radians()), positions ); } @@ -119,10 +125,10 @@ public static void run(double directionalX, double directionalY, double turn, in return; } - instance.odometry.update(new Rotation2d(Math.toRadians(Pigeon.getYaw())), instance.positions); + instance.odometry.update(new Rotation2d(Pigeon.getYaw().radians()), instance.positions); if (lowSense != -1) { - double angle = Math.TAU - (Math.toRadians((double)lowSense)); + double angle = Math.TAU - Math.toRadians((double)lowSense); // inverted since the drive is rotated to compensate for joystick stuff directionalX = -(Math.sin(angle) * LOW_SENSITIVITY_RATIO); @@ -139,7 +145,7 @@ public static void run(double directionalX, double directionalY, double turn, in ChassisSpeeds chassisSpeeds = instance.mode == SwerveMode.Headless ? ChassisSpeeds.fromFieldRelativeSpeeds( directionalX, -directionalY, turn, - new Rotation2d(Math.toRadians(Pigeon.getYaw())) + new Rotation2d(Pigeon.getYaw().radians()) ) : new ChassisSpeeds(directionalX, -directionalY, turn); @@ -166,7 +172,7 @@ public static void runUncurved(double directionalX, double directionalY, double ChassisSpeeds chassisSpeeds = instance.mode == SwerveMode.Headless ? ChassisSpeeds.fromFieldRelativeSpeeds( directionalX, -directionalY, turn, - new Rotation2d(Math.toRadians(Pigeon.getYaw())) + new Rotation2d(Pigeon.getYaw().radians()) ) : new ChassisSpeeds(directionalX, -directionalY, turn); @@ -182,7 +188,7 @@ public static void runUncurved(double directionalX, double directionalY, double */ public static void runRockMode() { for (int i = 0; i < instance.modules.length; i++) { - instance.modules[i].setDesiredState(new SwerveModuleState(0.0, new Rotation2d(MODULE_ROCK_MODE_PSOITIONS[i]))); + instance.modules[i].setDesiredState(new SwerveModuleState(0.0, new Rotation2d(MODULE_ROCK_MODE_POSITIONS[i]))); } } From decab735c4a762fadc6bcd7445ababa284ddc0c3 Mon Sep 17 00:00:00 2001 From: Evan Overman Date: Sun, 28 May 2023 13:21:15 -0700 Subject: [PATCH 06/11] have modules id by position rather than id --- .../frc/robot/subsystems/SwerveModule.java | 84 +++++++++++++++---- .../java/frc/robot/systems/SwerveDrive.java | 35 ++++++-- 2 files changed, 96 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index b68c5f1..d2e759b 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -14,6 +14,7 @@ import edu.wpi.first.math.controller.PIDController; 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; @@ -49,6 +50,7 @@ public class SwerveModule extends SmartPrintable { private final PIDController rotationController; private final PIDController rockController; + private final RelativePosition physicalPosition; private final Angle canCoderOffset; private final double coefficient; @@ -58,9 +60,65 @@ public class SwerveModule extends SmartPrintable { private double rockPos = Double.NaN; private double maxSpeed = Double.NaN; - public SwerveModule(int movementMotorID, int rotationalMotorID, int canCoderID, Angle canCoderOffset, double coefficient) { + 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; + } + } + + public SwerveModule( + int movementMotorID, + int rotationalMotorID, + int canCoderID, + Angle canCoderOffset, + double coefficient, + Translation2d physicalPosition + ) { super(); + this.physicalPosition = RelativePosition.fromTranslation(physicalPosition); this.canCoderOffset = canCoderOffset.clone(); this.coefficient = coefficient; @@ -111,7 +169,8 @@ public SwerveModule(int movementMotorID, int rotationalMotorID, int canCoderID, } /** - * Sets the desired module state for this module. + * 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()) % Math.TAU; @@ -123,6 +182,8 @@ public void setDesiredState(SwerveModuleState state) { ? Math.signum(state.speedMetersPerSecond) * maxSpeed : state.speedMetersPerSecond ); + } else { + movementMotor.set(rockController.calculate(getDistanceTraveled(), rockPos)); } double calculation = rotationController.calculate(currentPosition, (state.angle.getRadians() + Math.TAU) % Math.TAU); @@ -139,14 +200,9 @@ public void setRockMode(boolean shouldHold) { if (!shouldHold) { rockPos = Double.NaN; return; - } - - if (shouldHold && rockPos != rockPos) { + } else if (rockPos != rockPos) { rockPos = getDistanceTraveled(); } - - // TODO: move the running of rock mode elsewhere... - movementMotor.set(rockController.calculate(getDistanceTraveled(), rockPos)); } /** @@ -157,7 +213,7 @@ public boolean inRockMode() { } /** - * Sets this module's maximum movement speed + * Sets this module's maximum movement speed. Use NaN for no limit. */ public void setMaxSpeed(double maxSpeed) { this.maxSpeed = maxSpeed; @@ -182,11 +238,11 @@ public double getDistanceTraveled() { @Override public void print() { - SmartDashboard.putNumber("Module " + angularEncoder.getDeviceID() + " Position", angularEncoder.getPosition()); - SmartDashboard.putNumber("Module " + angularEncoder.getDeviceID() + " Position mod tau", angularEncoder.getPosition() % Math.TAU); - SmartDashboard.putNumber("Module " + angularEncoder.getDeviceID() + " Position + off", angularEncoder.getPosition() + canCoderOffset.radians()); - SmartDashboard.putNumber("Module " + angularEncoder.getDeviceID() + " Position + off mod tau", (angularEncoder.getPosition() + canCoderOffset.radians()) % Math.TAU); - SmartDashboard.putNumber("Module " + angularEncoder.getDeviceID() + " Position (Distance) ", movementEncoder.getPosition()); + SmartDashboard.putNumber("Module " + physicalPosition.asString() + " Position", angularEncoder.getPosition()); + SmartDashboard.putNumber("Module " + physicalPosition.asString() + " Position mod tau", angularEncoder.getPosition() % Math.TAU); + SmartDashboard.putNumber("Module " + physicalPosition.asString() + " Position + off", angularEncoder.getPosition() + canCoderOffset.radians()); + SmartDashboard.putNumber("Module " + physicalPosition.asString() + " Position + off mod tau", (angularEncoder.getPosition() + canCoderOffset.radians()) % Math.TAU); + SmartDashboard.putNumber("Module " + physicalPosition.asString() + " Position (Distance) ", movementEncoder.getPosition()); } /** diff --git a/src/main/java/frc/robot/systems/SwerveDrive.java b/src/main/java/frc/robot/systems/SwerveDrive.java index 600509e..e4ccb6c 100644 --- a/src/main/java/frc/robot/systems/SwerveDrive.java +++ b/src/main/java/frc/robot/systems/SwerveDrive.java @@ -31,6 +31,10 @@ public class SwerveDrive extends SmartPrintable { 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 MODULE_COEFFIENTS[] = { -1.0, -1.0, -1.0, -1.0 }; + private static final double LOW_SENSITIVITY_RATIO = 0.08; + 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), @@ -45,9 +49,12 @@ public class SwerveDrive extends SmartPrintable { new Angle().setRadians( Math.PI / 4) }; - private static final double MODULE_COEFFIENTS[] = { -1.0, -1.0, -1.0, -1.0 }; - private static final double LOW_SENSITIVITY_RATIO = 0.08; - private static final double CHASSIS_SIDE_LENGTH = 0.6; + 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(); @@ -73,7 +80,8 @@ private SwerveDrive() { MODULE_ROTATION_CAN_IDS[i], MODULE_CANCODER_CAN_IDS[i], MODULE_CANCODER_OFFSETS[i], - MODULE_COEFFIENTS[i] + MODULE_COEFFIENTS[i], + MODULE_PHYSICAL_POSITIONS[i] ); } @@ -82,10 +90,10 @@ private SwerveDrive() { } kinematics = new SwerveDriveKinematics( - 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) + MODULE_PHYSICAL_POSITIONS[0], + MODULE_PHYSICAL_POSITIONS[1], + MODULE_PHYSICAL_POSITIONS[2], + MODULE_PHYSICAL_POSITIONS[3] ); odometry = new SwerveDriveOdometry( @@ -188,7 +196,7 @@ public static void runUncurved(double directionalX, double directionalY, double */ public static void runRockMode() { for (int i = 0; i < instance.modules.length; i++) { - instance.modules[i].setDesiredState(new SwerveModuleState(0.0, new Rotation2d(MODULE_ROCK_MODE_POSITIONS[i]))); + instance.modules[i].setDesiredState(new SwerveModuleState(0.0, new Rotation2d(MODULE_ROCK_MODE_POSITIONS[i].radians()))); } } @@ -214,6 +222,15 @@ public static boolean getRockMode() { return rockMode; } + /** + * 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); + } + } + /** * Sets the curve function for directional inputs (translations). * @param curve The BiFunction to use for proccessing the curve, the first From 142b0e954eb1d4ab6fe600565448cc27386cd963 Mon Sep 17 00:00:00 2001 From: Evan Overman Date: Mon, 29 May 2023 11:50:54 -0700 Subject: [PATCH 07/11] rename `SwerveMode` variants to match naming convention --- src/main/java/frc/robot/Robot.java | 16 ++++++++-------- .../java/frc/robot/subsystems/SwerveMode.java | 4 ++-- src/main/java/frc/robot/systems/SwerveDrive.java | 6 +++--- 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 246540c..164f9b7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -144,7 +144,7 @@ public void autonomousPeriodic() { @Override public void teleopInit() { controlMode = ControlMode.TELEOPERATED; - SwerveDrive.setMode(SwerveMode.Headless); + SwerveDrive.setMode(SwerveMode.HEADLESS); ElevFourbar.init(); Intake.init(); ElevFourbar.enableSafetyMode(false); @@ -174,10 +174,10 @@ public void teleopPeriodic() { // Left stick changes between headless and relative control modes. if (controllerOne.getLeftStickButtonReleased()) { - if (SwerveDrive.getMode() == SwerveMode.Headless) { - SwerveDrive.setMode(SwerveMode.Relative); + if (SwerveDrive.getMode() == SwerveMode.HEADLESS) { + SwerveDrive.setMode(SwerveMode.RELATIVE); } else { - SwerveDrive.setMode(SwerveMode.Headless); + SwerveDrive.setMode(SwerveMode.HEADLESS); } } @@ -225,7 +225,7 @@ public void disabledPeriodic() { @Override public void testInit() { controlMode = ControlMode.SAFETY; - SwerveDrive.setMode(SwerveMode.Headless); + SwerveDrive.setMode(SwerveMode.HEADLESS); ElevFourbar.init(); Intake.init(); ElevFourbar.enableSafetyMode(true); @@ -251,10 +251,10 @@ public void testPeriodic() { // Left stick changes between headless and relative control modes. if (controllerOne.getLeftStickButtonReleased()) { - if (SwerveDrive.getMode() == SwerveMode.Headless) { - SwerveDrive.setMode(SwerveMode.Relative); + if (SwerveDrive.getMode() == SwerveMode.HEADLESS) { + SwerveDrive.setMode(SwerveMode.RELATIVE); } else { - SwerveDrive.setMode(SwerveMode.Headless); + SwerveDrive.setMode(SwerveMode.HEADLESS); } } diff --git a/src/main/java/frc/robot/subsystems/SwerveMode.java b/src/main/java/frc/robot/subsystems/SwerveMode.java index 278e52a..737080f 100644 --- a/src/main/java/frc/robot/subsystems/SwerveMode.java +++ b/src/main/java/frc/robot/subsystems/SwerveMode.java @@ -9,11 +9,11 @@ public enum SwerveMode { * Moves relative to the driver station, pressing the joystick away from * you should always move the robot away from you. */ - Headless, + HEADLESS, /** * Pressing the joystick forward moves the robot froward, regardless of * position relative to driver. */ - Relative + RELATIVE } diff --git a/src/main/java/frc/robot/systems/SwerveDrive.java b/src/main/java/frc/robot/systems/SwerveDrive.java index e4ccb6c..15d01a0 100644 --- a/src/main/java/frc/robot/systems/SwerveDrive.java +++ b/src/main/java/frc/robot/systems/SwerveDrive.java @@ -69,7 +69,7 @@ public class SwerveDrive extends SmartPrintable { // Variable drive states objects. private BiFunction directionCurve = Controls::defaultCurveTwoDimensional; private Function turnCurve = Controls::defaultCurve; - private SwerveMode mode = SwerveMode.Headless; + private SwerveMode mode = SwerveMode.HEADLESS; private SwerveDrive() { super(); @@ -150,7 +150,7 @@ public static void run(double directionalX, double directionalY, double turn, in directionalY = instance.directionCurve.apply(directionalY, directionalX); turn = instance.turnCurve.apply(turn); - ChassisSpeeds chassisSpeeds = instance.mode == SwerveMode.Headless + ChassisSpeeds chassisSpeeds = instance.mode == SwerveMode.HEADLESS ? ChassisSpeeds.fromFieldRelativeSpeeds( directionalX, -directionalY, turn, new Rotation2d(Pigeon.getYaw().radians()) @@ -177,7 +177,7 @@ public static void runUncurved(double directionalX, double directionalY, double return; } - ChassisSpeeds chassisSpeeds = instance.mode == SwerveMode.Headless + ChassisSpeeds chassisSpeeds = instance.mode == SwerveMode.HEADLESS ? ChassisSpeeds.fromFieldRelativeSpeeds( directionalX, -directionalY, turn, new Rotation2d(Pigeon.getYaw().radians()) From 1eeec43568d105519617726e4c1b2775ec6b4428 Mon Sep 17 00:00:00 2001 From: Evan Overman Date: Thu, 31 Aug 2023 23:04:22 -0700 Subject: [PATCH 08/11] see "The Tau Manifesto" for why Pi is a mistake --- src/main/java/frc/robot/subsystems/Angle.java | 6 ++++-- src/main/java/frc/robot/subsystems/SwerveModule.java | 10 +++++----- src/main/java/frc/robot/systems/SwerveDrive.java | 2 +- 3 files changed, 10 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Angle.java b/src/main/java/frc/robot/subsystems/Angle.java index 8bd03ca..e695a93 100644 --- a/src/main/java/frc/robot/subsystems/Angle.java +++ b/src/main/java/frc/robot/subsystems/Angle.java @@ -5,10 +5,12 @@ package frc.robot.subsystems; public final class Angle { + public static final double TAU = Math.PI * 2; + private double dec = 0.0; public double radians() { - return dec * Math.TAU; + return dec * TAU; } public double degrees() { @@ -16,7 +18,7 @@ public double degrees() { } public Angle setRadians(double radians) { - dec = radians / Math.TAU; + dec = radians / TAU; return this; } diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index d2e759b..a5f1449 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -156,7 +156,7 @@ public SwerveModule( movementEncoder.setVelocityConversionFactor(CONVERSION_FACTOR_MOVEMENT_VELOCITY); rotationController = new PIDController(PID_P, PID_I, PID_D); - rotationController.enableContinuousInput(0.0, Math.TAU); + rotationController.enableContinuousInput(0.0, Angle.TAU); rotationController.setTolerance(0.005); // This is more precise than before, TODO: test. rockController = new PIDController(ROCK_PID_P, ROCK_PID_I, ROCK_PID_D); @@ -173,7 +173,7 @@ public SwerveModule( * repeatedly to continue PID calculations. */ public void setDesiredState(SwerveModuleState state) { - double currentPosition = (angularEncoder.getPosition() + canCoderOffset.radians()) % Math.TAU; + double currentPosition = (angularEncoder.getPosition() + canCoderOffset.radians()) % Angle.TAU; state = SwerveModuleState.optimize(state, new Rotation2d(currentPosition)); if (rockPos != rockPos) { @@ -186,7 +186,7 @@ public void setDesiredState(SwerveModuleState state) { movementMotor.set(rockController.calculate(getDistanceTraveled(), rockPos)); } - double calculation = rotationController.calculate(currentPosition, (state.angle.getRadians() + Math.TAU) % Math.TAU); + double calculation = rotationController.calculate(currentPosition, (state.angle.getRadians() + Angle.TAU) % Angle.TAU); rotationMotor.set(calculation * coefficient); position.angle = new Rotation2d(angularEncoder.getPosition()); @@ -239,9 +239,9 @@ public double getDistanceTraveled() { @Override public void print() { SmartDashboard.putNumber("Module " + physicalPosition.asString() + " Position", angularEncoder.getPosition()); - SmartDashboard.putNumber("Module " + physicalPosition.asString() + " Position mod tau", angularEncoder.getPosition() % Math.TAU); + SmartDashboard.putNumber("Module " + physicalPosition.asString() + " Position mod tau", angularEncoder.getPosition() % Angle.TAU); SmartDashboard.putNumber("Module " + physicalPosition.asString() + " Position + off", angularEncoder.getPosition() + canCoderOffset.radians()); - SmartDashboard.putNumber("Module " + physicalPosition.asString() + " Position + off mod tau", (angularEncoder.getPosition() + canCoderOffset.radians()) % Math.TAU); + SmartDashboard.putNumber("Module " + physicalPosition.asString() + " Position + off mod tau", (angularEncoder.getPosition() + canCoderOffset.radians()) % Angle.TAU); SmartDashboard.putNumber("Module " + physicalPosition.asString() + " Position (Distance) ", movementEncoder.getPosition()); } diff --git a/src/main/java/frc/robot/systems/SwerveDrive.java b/src/main/java/frc/robot/systems/SwerveDrive.java index 15d01a0..8184c68 100644 --- a/src/main/java/frc/robot/systems/SwerveDrive.java +++ b/src/main/java/frc/robot/systems/SwerveDrive.java @@ -136,7 +136,7 @@ public static void run(double directionalX, double directionalY, double turn, in instance.odometry.update(new Rotation2d(Pigeon.getYaw().radians()), instance.positions); if (lowSense != -1) { - double angle = Math.TAU - Math.toRadians((double)lowSense); + double angle = Angle.TAU - Math.toRadians((double)lowSense); // inverted since the drive is rotated to compensate for joystick stuff directionalX = -(Math.sin(angle) * LOW_SENSITIVITY_RATIO); From c3bc40c11d8d1037888e72c15d7a67b3aa8a5dbd Mon Sep 17 00:00:00 2001 From: Evan Overman Date: Sat, 2 Sep 2023 12:23:43 -0700 Subject: [PATCH 09/11] rework rock mode interface --- src/main/java/frc/robot/Robot.java | 4 +- .../java/frc/robot/subsystems/SwerveMode.java | 7 +- .../frc/robot/systems/AutonomousRunner.java | 2 - .../java/frc/robot/systems/LEDLights.java | 3 +- .../java/frc/robot/systems/SwerveDrive.java | 144 ++++++++++-------- 5 files changed, 87 insertions(+), 73 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 164f9b7..2ae2ccc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -170,7 +170,7 @@ public void teleopPeriodic() { } SwerveDrive.run(x, y, controllerOne.getRightX(), controllerOne.getPOV()); - SwerveDrive.setRockMode(controllerOne.getRightTriggerAxis() > 0.25); + SwerveDrive.conditionalTempMode(SwerveMode.ROCK, controllerOne.getRightTriggerAxis() > 0.25); // Left stick changes between headless and relative control modes. if (controllerOne.getLeftStickButtonReleased()) { @@ -247,7 +247,7 @@ public void testPeriodic() { //SCALES DOWN DRIVE SPEED FOR SwerveDrive.run(x, y, controllerOne.getRightX(), controllerOne.getPOV()); - SwerveDrive.setRockMode(controllerOne.getRightTriggerAxis() > 0.25); + SwerveDrive.conditionalTempMode(SwerveMode.ROCK, controllerOne.getRightTriggerAxis() > 0.25); // Left stick changes between headless and relative control modes. if (controllerOne.getLeftStickButtonReleased()) { diff --git a/src/main/java/frc/robot/subsystems/SwerveMode.java b/src/main/java/frc/robot/subsystems/SwerveMode.java index 737080f..790fb8c 100644 --- a/src/main/java/frc/robot/subsystems/SwerveMode.java +++ b/src/main/java/frc/robot/subsystems/SwerveMode.java @@ -15,5 +15,10 @@ public enum SwerveMode { * Pressing the joystick forward moves the robot froward, regardless of * position relative to driver. */ - RELATIVE + RELATIVE, + + /** + * Rock mode, resists movement and holds position. + */ + ROCK } diff --git a/src/main/java/frc/robot/systems/AutonomousRunner.java b/src/main/java/frc/robot/systems/AutonomousRunner.java index bca9a66..4dcde27 100644 --- a/src/main/java/frc/robot/systems/AutonomousRunner.java +++ b/src/main/java/frc/robot/systems/AutonomousRunner.java @@ -66,8 +66,6 @@ public static void init() { } public static void run() { - SwerveDrive.setRockMode(false); - SmartDashboard.putNumber("Auto Stage", instance.autoStage); //Run the auto mode diff --git a/src/main/java/frc/robot/systems/LEDLights.java b/src/main/java/frc/robot/systems/LEDLights.java index 4632ab3..c879b3c 100644 --- a/src/main/java/frc/robot/systems/LEDLights.java +++ b/src/main/java/frc/robot/systems/LEDLights.java @@ -8,6 +8,7 @@ import frc.robot.patterns.FadeIntoPattern; import frc.robot.subsystems.LightPattern; import frc.robot.subsystems.LightRenderer; +import frc.robot.subsystems.SwerveMode; import frc.robot.systems.ElevFourbar.GamePiece; public class LEDLights { @@ -42,7 +43,7 @@ public static void run() { } else if (Robot.getControlMode() == ControlMode.AUTONOMOUS) { setPattern = new FadeIn(new Color(0.0, 1.0, 0.0), 1.0); } else { - if (SwerveDrive.getRockMode()) { + if (SwerveDrive.getMode() == SwerveMode.ROCK) { // If in rock mode make wyvern scary >:D setPattern = new FadeIn(new Color(1.0, 0.0, 0.0), 1.0); } else { diff --git a/src/main/java/frc/robot/systems/SwerveDrive.java b/src/main/java/frc/robot/systems/SwerveDrive.java index 8184c68..14a27a1 100644 --- a/src/main/java/frc/robot/systems/SwerveDrive.java +++ b/src/main/java/frc/robot/systems/SwerveDrive.java @@ -70,6 +70,7 @@ public class SwerveDrive extends SmartPrintable { private BiFunction directionCurve = Controls::defaultCurveTwoDimensional; private Function turnCurve = Controls::defaultCurve; private SwerveMode mode = SwerveMode.HEADLESS; + private SwerveMode inactiveMode = SwerveMode.HEADLESS; private SwerveDrive() { super(); @@ -119,22 +120,48 @@ public static SwerveMode getMode() { return instance.mode; } + /** + * 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.mode != 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); + } + /** * Runs swerve, behavior changes based on the drive's mode. Derives speed - * from directional inputs. + * from directional inputs. This will reset temporary modes on completion. * @param directionalX The X axis of the directional control, between 1 and -1 * @param directionalY The Y axis of the directional control, between 1 and -1. * @param turn A value between 1 and -1 that determines the turning angle. * @param lowSense The angle to move in low sensitivity in degrees, -1 for no movement. */ public static void run(double directionalX, double directionalY, double turn, int lowSense) { - if (getRockMode()) { - runRockMode(); - return; - } - - instance.odometry.update(new Rotation2d(Pigeon.getYaw().radians()), instance.positions); - if (lowSense != -1) { double angle = Angle.TAU - Math.toRadians((double)lowSense); @@ -149,77 +176,61 @@ public static void run(double directionalX, double directionalY, double turn, in directionalX = instance.directionCurve.apply(directionalX, directionalY); directionalY = instance.directionCurve.apply(directionalY, directionalX); turn = instance.turnCurve.apply(turn); - - ChassisSpeeds chassisSpeeds = instance.mode == SwerveMode.HEADLESS - ? ChassisSpeeds.fromFieldRelativeSpeeds( - directionalX, -directionalY, turn, - new Rotation2d(Pigeon.getYaw().radians()) - ) - : new ChassisSpeeds(directionalX, -directionalY, turn); - - SwerveModuleState[] moduleStates = instance.kinematics.toSwerveModuleStates(chassisSpeeds); - - for (int i = 0; i < instance.modules.length; i++) { - instance.modules[i].setDesiredState(moduleStates[i]); - } + runUncurved(directionalX, directionalY, turn); } - + /** - * Run the swerve drive exactly by the arguments passed, no cuving will - * occure on the given inputs, nor will the be deadbanded. + * 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 directionalX Speed along the x axis. -1.0 - 1.0 * @param directionalY Speed along the y axis. -1.0 - 1.0 * @param turn Rate of turn. -1.0 - 1.0 */ public static void runUncurved(double directionalX, double directionalY, double turn) { - if (getRockMode()) { - runRockMode(); - return; - } + SwerveModuleState[] moduleStates = new SwerveModuleState[instance.modules.length]; + boolean holdPos = false; + + switch (instance.mode) { + case HEADLESS: + moduleStates = instance.kinematics.toSwerveModuleStates( + ChassisSpeeds.fromFieldRelativeSpeeds( + directionalX, -directionalY, turn, + new Rotation2d(Pigeon.getYaw().radians()) + ) + ); + break; + + case RELATIVE: + moduleStates = instance.kinematics.toSwerveModuleStates( + new ChassisSpeeds(directionalX, -directionalY, turn) + ); + 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())); + } - ChassisSpeeds chassisSpeeds = instance.mode == SwerveMode.HEADLESS - ? ChassisSpeeds.fromFieldRelativeSpeeds( - directionalX, -directionalY, turn, - new Rotation2d(Pigeon.getYaw().radians()) - ) - : new ChassisSpeeds(directionalX, -directionalY, turn); + return; + } - SwerveModuleState[] moduleStates = instance.kinematics.toSwerveModuleStates(chassisSpeeds); + // 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); } - } - /** - * Wyvern becomes rock, rock do not move, Wyvern do not move... - */ - public static void runRockMode() { - for (int i = 0; i < instance.modules.length; i++) { - instance.modules[i].setDesiredState(new SwerveModuleState(0.0, new Rotation2d(MODULE_ROCK_MODE_POSITIONS[i].radians()))); - } - } - - /** - * True if rock mode should be active. - */ - public static void setRockMode(boolean shouldHold) { - for (SwerveModule module : instance.modules) { - module.setRockMode(shouldHold); - } - } - - /** - * Returns true if in rock mode. - */ - public static boolean getRockMode() { - boolean rockMode = false; - - for (SwerveModule module : instance.modules) { - rockMode |= module.inRockMode(); - } - - return rockMode; + instance.odometry.update(new Rotation2d(Pigeon.getYaw().radians()), instance.positions); + instance.mode = instance.inactiveMode; + instance.inactiveMode = null; } /** @@ -313,8 +324,7 @@ public static void setTurnCurve(Function curve) { */ @Override public void print() { - SmartDashboard.putString("Orientation", getMode().toString()); - SmartDashboard.putBoolean("In Rock Mode", getRockMode()); + SmartDashboard.putString("Swerve Mode", getMode().toString()); } /** From a7459d028489e9e1997708b174a7f8fd7be443d3 Mon Sep 17 00:00:00 2001 From: Evan Overman Date: Thu, 7 Sep 2023 22:59:55 -0700 Subject: [PATCH 10/11] fix null ref --- src/main/java/frc/robot/systems/SwerveDrive.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/systems/SwerveDrive.java b/src/main/java/frc/robot/systems/SwerveDrive.java index 14a27a1..de29d43 100644 --- a/src/main/java/frc/robot/systems/SwerveDrive.java +++ b/src/main/java/frc/robot/systems/SwerveDrive.java @@ -229,8 +229,11 @@ public static void runUncurved(double directionalX, double directionalY, double } instance.odometry.update(new Rotation2d(Pigeon.getYaw().radians()), instance.positions); - instance.mode = instance.inactiveMode; - instance.inactiveMode = null; + + if (instance.inactiveMode != null) { + instance.mode = instance.inactiveMode; + instance.inactiveMode = null; + } } /** From 6dc703af499c347494e22c29c68569e4082f2a9f Mon Sep 17 00:00:00 2001 From: Evan Overman Date: Thu, 7 Sep 2023 23:42:34 -0700 Subject: [PATCH 11/11] bug fixes --- src/main/java/frc/robot/subsystems/SwerveModule.java | 3 ++- src/main/java/frc/robot/systems/SwerveDrive.java | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index a5f1449..848bff3 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -30,7 +30,7 @@ public class SwerveModule extends SmartPrintable { 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 PID_P = 0.01; + private static final double PID_P = 0.5; private static final double PID_I = 0.0; private static final double PID_D = 0.0; @@ -132,6 +132,7 @@ public SwerveModule( movementMotor.setSmartCurrentLimit(40); angularEncoder = new CANCoder(canCoderID); + angularEncoder.configFactoryDefault(); // Magic and forbidden config from the code orange wizards. Makes the // encoder initialize to absolute. diff --git a/src/main/java/frc/robot/systems/SwerveDrive.java b/src/main/java/frc/robot/systems/SwerveDrive.java index de29d43..356ec5f 100644 --- a/src/main/java/frc/robot/systems/SwerveDrive.java +++ b/src/main/java/frc/robot/systems/SwerveDrive.java @@ -132,7 +132,7 @@ public static SwerveMode getMode() { * running will not be effected */ public static void tempMode(SwerveMode mode) { - if (instance.mode != null) { + if (instance.inactiveMode != null) { instance.mode = mode; return; } @@ -215,7 +215,7 @@ public static void runUncurved(double directionalX, double directionalY, double moduleStates[i] = new SwerveModuleState(0.0, new Rotation2d(MODULE_ROCK_MODE_POSITIONS[i].radians())); } - return; + break; } // This branch should never be reached as the enum used should never