From c615ec488cc8449f517490d2571e1eac44d1b261 Mon Sep 17 00:00:00 2001 From: DriverStation3 Date: Thu, 4 Jan 2024 20:00:51 -0800 Subject: [PATCH 01/12] get a start to drivebase subsystem --- .../practiceswerve/controllerproperties.json | 8 + .../practiceswerve/modules/backleft.json | 30 ++ .../practiceswerve/modules/backright.json | 30 ++ .../practiceswerve/modules/frontleft.json | 30 ++ .../practiceswerve/modules/frontright.json | 30 ++ .../modules/physicalproperties.json | 16 + .../modules/pidfproperties.json | 16 + .../deploy/practiceswerve/swervedrive.json | 14 + .../deploy/swerve/controllerproperties.json | 8 + src/main/deploy/swerve/modules/backleft.json | 30 ++ src/main/deploy/swerve/modules/backright.json | 30 ++ src/main/deploy/swerve/modules/frontleft.json | 30 ++ .../deploy/swerve/modules/frontright.json | 30 ++ .../swerve/modules/physicalproperties.json | 16 + .../deploy/swerve/modules/pidfproperties.json | 16 + src/main/deploy/swerve/swervedrive.json | 14 + src/main/java/frc/team2412/robot/Robot.java | 35 +- .../robot/subsystems/DrivebaseSubsystem.java | 79 ++++ .../frc/team2412/robot/util/MACAddress.java | 98 +++++ vendordeps/PathplannerLib.json | 38 ++ vendordeps/Phoenix6.json | 339 ++++++++++++++++++ vendordeps/yagsl.json | 19 + 22 files changed, 954 insertions(+), 2 deletions(-) create mode 100644 src/main/deploy/practiceswerve/controllerproperties.json create mode 100644 src/main/deploy/practiceswerve/modules/backleft.json create mode 100644 src/main/deploy/practiceswerve/modules/backright.json create mode 100644 src/main/deploy/practiceswerve/modules/frontleft.json create mode 100644 src/main/deploy/practiceswerve/modules/frontright.json create mode 100644 src/main/deploy/practiceswerve/modules/physicalproperties.json create mode 100644 src/main/deploy/practiceswerve/modules/pidfproperties.json create mode 100644 src/main/deploy/practiceswerve/swervedrive.json create mode 100644 src/main/deploy/swerve/controllerproperties.json create mode 100644 src/main/deploy/swerve/modules/backleft.json create mode 100644 src/main/deploy/swerve/modules/backright.json create mode 100644 src/main/deploy/swerve/modules/frontleft.json create mode 100644 src/main/deploy/swerve/modules/frontright.json create mode 100644 src/main/deploy/swerve/modules/physicalproperties.json create mode 100644 src/main/deploy/swerve/modules/pidfproperties.json create mode 100644 src/main/deploy/swerve/swervedrive.json create mode 100644 src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java create mode 100644 src/main/java/frc/team2412/robot/util/MACAddress.java create mode 100644 vendordeps/PathplannerLib.json create mode 100644 vendordeps/Phoenix6.json create mode 100644 vendordeps/yagsl.json diff --git a/src/main/deploy/practiceswerve/controllerproperties.json b/src/main/deploy/practiceswerve/controllerproperties.json new file mode 100644 index 00000000..c40df9f9 --- /dev/null +++ b/src/main/deploy/practiceswerve/controllerproperties.json @@ -0,0 +1,8 @@ +{ + "angleJoystickRadiusDeadband": 0.5, + "heading": { + "p": 0.4, + "i": 0, + "d": 0 + } +} \ No newline at end of file diff --git a/src/main/deploy/practiceswerve/modules/backleft.json b/src/main/deploy/practiceswerve/modules/backleft.json new file mode 100644 index 00000000..a59f11fa --- /dev/null +++ b/src/main/deploy/practiceswerve/modules/backleft.json @@ -0,0 +1,30 @@ +{ + "location": { + "front": -8.5, + "left": 8.5 + }, + "absoluteEncoderOffset": 206.191, + "drive": { + "type": "falcon", + "id": 7, + "canbus": null + }, + "angle": { + "type": "falcon", + "id": 8, + "canbus": null + }, + "conversionFactor": { + "angle": 0.01373291, + "drive": 0.000019146492 + }, + "encoder": { + "type": "cancoder", + "id": 9, + "canbus": null + }, + "inverted": { + "drive": false, + "angle": false + } +} \ No newline at end of file diff --git a/src/main/deploy/practiceswerve/modules/backright.json b/src/main/deploy/practiceswerve/modules/backright.json new file mode 100644 index 00000000..abcdbcc9 --- /dev/null +++ b/src/main/deploy/practiceswerve/modules/backright.json @@ -0,0 +1,30 @@ +{ + "location": { + "front": -8.5, + "left": -8.5 + }, + "absoluteEncoderOffset": 309.99, + "drive": { + "type": "falcon", + "id": 10, + "canbus": null + }, + "angle": { + "type": "falcon", + "id": 11, + "canbus": null + }, + "conversionFactor": { + "angle": 0.01373291, + "drive": 0.000019146492 + }, + "encoder": { + "type": "cancoder", + "id": 12, + "canbus": null + }, + "inverted": { + "drive": false, + "angle": false + } +} \ No newline at end of file diff --git a/src/main/deploy/practiceswerve/modules/frontleft.json b/src/main/deploy/practiceswerve/modules/frontleft.json new file mode 100644 index 00000000..6c00fe7c --- /dev/null +++ b/src/main/deploy/practiceswerve/modules/frontleft.json @@ -0,0 +1,30 @@ +{ + "location": { + "front": 8.5, + "left": 8.5 + }, + "absoluteEncoderOffset": 524.971, + "drive": { + "type": "falcon", + "id": 1, + "canbus": null + }, + "angle": { + "type": "falcon", + "id": 2, + "canbus": null + }, + "conversionFactor": { + "angle": 0.01373291, + "drive": 0.000019146492 + }, + "encoder": { + "type": "cancoder", + "id": 3, + "canbus": null + }, + "inverted": { + "drive": false, + "angle": false + } +} \ No newline at end of file diff --git a/src/main/deploy/practiceswerve/modules/frontright.json b/src/main/deploy/practiceswerve/modules/frontright.json new file mode 100644 index 00000000..33abccf2 --- /dev/null +++ b/src/main/deploy/practiceswerve/modules/frontright.json @@ -0,0 +1,30 @@ +{ + "location": { + "front": 8.5, + "left": -8.5 + }, + "absoluteEncoderOffset": 249.346, + "drive": { + "type": "falcon", + "id": 4, + "canbus": null + }, + "angle": { + "type": "falcon", + "id": 5, + "canbus": null + }, + "conversionFactor": { + "angle": 0.01373291, + "drive": 0.000019146492 + }, + "encoder": { + "type": "cancoder", + "id": 6, + "canbus": null + }, + "inverted": { + "drive": false, + "angle": false + } +} \ No newline at end of file diff --git a/src/main/deploy/practiceswerve/modules/physicalproperties.json b/src/main/deploy/practiceswerve/modules/physicalproperties.json new file mode 100644 index 00000000..288ab51e --- /dev/null +++ b/src/main/deploy/practiceswerve/modules/physicalproperties.json @@ -0,0 +1,16 @@ +{ + "optimalVoltage": 12, + "wheelGripCoefficientOfFriction": 1.19, + "currentLimit": { + "drive": 40, + "angle": 20 + }, + "conversionFactor": { + "angle": 0.01373291, + "drive": 0.000019146492 + }, + "rampRate": { + "drive": 0.25, + "angle": 0.25 + } +} \ No newline at end of file diff --git a/src/main/deploy/practiceswerve/modules/pidfproperties.json b/src/main/deploy/practiceswerve/modules/pidfproperties.json new file mode 100644 index 00000000..3834a362 --- /dev/null +++ b/src/main/deploy/practiceswerve/modules/pidfproperties.json @@ -0,0 +1,16 @@ +{ + "drive": { + "p": 0.0020645, + "i": 0, + "d": 0, + "f": 0, + "iz": 0 + }, + "angle": { + "p": 0.0020645, + "i": 0, + "d": 0, + "f": 0, + "iz": 0 + } +} \ No newline at end of file diff --git a/src/main/deploy/practiceswerve/swervedrive.json b/src/main/deploy/practiceswerve/swervedrive.json new file mode 100644 index 00000000..d2b6dbd5 --- /dev/null +++ b/src/main/deploy/practiceswerve/swervedrive.json @@ -0,0 +1,14 @@ +{ + "imu": { + "type": "navx_mxp", + "id": 0, + "canbus": null + }, + "invertedIMU": true, + "modules": [ + "frontleft.json", + "frontright.json", + "backleft.json", + "backright.json" + ] +} \ No newline at end of file diff --git a/src/main/deploy/swerve/controllerproperties.json b/src/main/deploy/swerve/controllerproperties.json new file mode 100644 index 00000000..c40df9f9 --- /dev/null +++ b/src/main/deploy/swerve/controllerproperties.json @@ -0,0 +1,8 @@ +{ + "angleJoystickRadiusDeadband": 0.5, + "heading": { + "p": 0.4, + "i": 0, + "d": 0 + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/backleft.json b/src/main/deploy/swerve/modules/backleft.json new file mode 100644 index 00000000..a59f11fa --- /dev/null +++ b/src/main/deploy/swerve/modules/backleft.json @@ -0,0 +1,30 @@ +{ + "location": { + "front": -8.5, + "left": 8.5 + }, + "absoluteEncoderOffset": 206.191, + "drive": { + "type": "falcon", + "id": 7, + "canbus": null + }, + "angle": { + "type": "falcon", + "id": 8, + "canbus": null + }, + "conversionFactor": { + "angle": 0.01373291, + "drive": 0.000019146492 + }, + "encoder": { + "type": "cancoder", + "id": 9, + "canbus": null + }, + "inverted": { + "drive": false, + "angle": false + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/backright.json b/src/main/deploy/swerve/modules/backright.json new file mode 100644 index 00000000..abcdbcc9 --- /dev/null +++ b/src/main/deploy/swerve/modules/backright.json @@ -0,0 +1,30 @@ +{ + "location": { + "front": -8.5, + "left": -8.5 + }, + "absoluteEncoderOffset": 309.99, + "drive": { + "type": "falcon", + "id": 10, + "canbus": null + }, + "angle": { + "type": "falcon", + "id": 11, + "canbus": null + }, + "conversionFactor": { + "angle": 0.01373291, + "drive": 0.000019146492 + }, + "encoder": { + "type": "cancoder", + "id": 12, + "canbus": null + }, + "inverted": { + "drive": false, + "angle": false + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/frontleft.json b/src/main/deploy/swerve/modules/frontleft.json new file mode 100644 index 00000000..6c00fe7c --- /dev/null +++ b/src/main/deploy/swerve/modules/frontleft.json @@ -0,0 +1,30 @@ +{ + "location": { + "front": 8.5, + "left": 8.5 + }, + "absoluteEncoderOffset": 524.971, + "drive": { + "type": "falcon", + "id": 1, + "canbus": null + }, + "angle": { + "type": "falcon", + "id": 2, + "canbus": null + }, + "conversionFactor": { + "angle": 0.01373291, + "drive": 0.000019146492 + }, + "encoder": { + "type": "cancoder", + "id": 3, + "canbus": null + }, + "inverted": { + "drive": false, + "angle": false + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/frontright.json b/src/main/deploy/swerve/modules/frontright.json new file mode 100644 index 00000000..33abccf2 --- /dev/null +++ b/src/main/deploy/swerve/modules/frontright.json @@ -0,0 +1,30 @@ +{ + "location": { + "front": 8.5, + "left": -8.5 + }, + "absoluteEncoderOffset": 249.346, + "drive": { + "type": "falcon", + "id": 4, + "canbus": null + }, + "angle": { + "type": "falcon", + "id": 5, + "canbus": null + }, + "conversionFactor": { + "angle": 0.01373291, + "drive": 0.000019146492 + }, + "encoder": { + "type": "cancoder", + "id": 6, + "canbus": null + }, + "inverted": { + "drive": false, + "angle": false + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/physicalproperties.json b/src/main/deploy/swerve/modules/physicalproperties.json new file mode 100644 index 00000000..288ab51e --- /dev/null +++ b/src/main/deploy/swerve/modules/physicalproperties.json @@ -0,0 +1,16 @@ +{ + "optimalVoltage": 12, + "wheelGripCoefficientOfFriction": 1.19, + "currentLimit": { + "drive": 40, + "angle": 20 + }, + "conversionFactor": { + "angle": 0.01373291, + "drive": 0.000019146492 + }, + "rampRate": { + "drive": 0.25, + "angle": 0.25 + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/pidfproperties.json b/src/main/deploy/swerve/modules/pidfproperties.json new file mode 100644 index 00000000..3834a362 --- /dev/null +++ b/src/main/deploy/swerve/modules/pidfproperties.json @@ -0,0 +1,16 @@ +{ + "drive": { + "p": 0.0020645, + "i": 0, + "d": 0, + "f": 0, + "iz": 0 + }, + "angle": { + "p": 0.0020645, + "i": 0, + "d": 0, + "f": 0, + "iz": 0 + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/swervedrive.json b/src/main/deploy/swerve/swervedrive.json new file mode 100644 index 00000000..d2b6dbd5 --- /dev/null +++ b/src/main/deploy/swerve/swervedrive.json @@ -0,0 +1,14 @@ +{ + "imu": { + "type": "navx_mxp", + "id": 0, + "canbus": null + }, + "invertedIMU": true, + "modules": [ + "frontleft.json", + "frontright.json", + "backleft.json", + "backright.json" + ] +} \ No newline at end of file diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index bd3c2fce..7ef07987 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -1,26 +1,49 @@ package frc.team2412.robot; -import edu.wpi.first.wpilibj.*; +import edu.wpi.first.wpilibj.DataLogManager; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.team2412.robot.util.MACAddress; public class Robot extends TimedRobot { /** Singleton Stuff */ private static Robot instance = null; + enum RobotType { + COMPETITION, + PRACTICE; + } + public static Robot getInstance() { if (instance == null) instance = new Robot(); return instance; } + private final RobotType robotType; public Controls controls; public Subsystems subsystems; - protected Robot() { + protected Robot(RobotType type) { // non public for singleton. Protected so test class can subclass instance = this; + robotType = type; + } + + protected Robot() { + this(getTypeFromAddress()); + } + + public static final MACAddress COMPETITION_ADDRESS = MACAddress.of(0x33, 0x9d, 0xd1); + public static final MACAddress PRACTICE_ADDRESS = MACAddress.of(0x33, 0x9D, 0xE7); + + private static RobotType getTypeFromAddress() { + if (PRACTICE_ADDRESS.exists()) return RobotType.PRACTICE; + else {return RobotType.COMPETITION;} } @Override @@ -86,4 +109,12 @@ public void teleopExit() { public void disabledInit() { Shuffleboard.stopRecording(); } + + public RobotType getRobotType() { + return robotType; + } + + public boolean isCompetition() { + return getRobotType() == RobotType.COMPETITION; + } } diff --git a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java new file mode 100644 index 00000000..fce7eafd --- /dev/null +++ b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java @@ -0,0 +1,79 @@ +package frc.team2412.robot.subsystems; + +import java.io.File; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.team2412.robot.Robot; +import swervelib.SwerveDrive; +import swervelib.parser.SwerveParser; + +public class DrivebaseSubsystem extends SubsystemBase { + + // SWERVE CONSTANTS (that aren't in deploy dir) + + private static final double MAX_SPEED = 1.0; + + private final SwerveDrive swerveDrive; + + public DrivebaseSubsystem() { + File swerveJsonDirectory; + + if (Robot.getInstance().isCompetition()) { + swerveJsonDirectory = new File(Filesystem.getDeployDirectory(), "swerve"); + } else { + swerveJsonDirectory = new File(Filesystem.getDeployDirectory(), "practiceswerve"); + } + + try { + swerveDrive = new SwerveParser(swerveJsonDirectory).createSwerveDrive(MAX_SPEED); + } catch (Exception e) { + throw new RuntimeException(); + } + + // set drive motors to brake + swerveDrive.setMotorIdleMode(true); + // enable optimization (never move the angle wheels more than 90 degrees) + swerveDrive.setModuleStateOptimization(true); + // swerve drive heading will slowly drift over time as you translate. this method enables an active correction using pid. disabled until testing can be done + swerveDrive.setHeadingCorrection(false); + // supposed to do something? see https://broncbotz3481.github.io/YAGSL/swervelib/SwerveDrive.html#chassisVelocityCorrection + swerveDrive.chassisVelocityCorrection = true; + + swerveDrive.synchronizeModuleEncoders(); + } + + public void drive( + Translation2d translation, + Rotation2d rotation, + boolean fieldOriented) { + swerveDrive.drive(translation, rotation.getRadians(), fieldOriented, false); + } + + /* + * Set the robot's pose. + * TODO: does this change yaw too? does this affect field oriented? + */ + public void setPose(Pose2d pose) { + swerveDrive.resetOdometry(pose); + } + + /* + * Reset the gyro angle. After this method is called, yaw will be zero. Pose is also updated to zero rotation but same position + */ + public void resetGyro() { + swerveDrive.zeroGyro(); + } + + /* + * Reset everything we can on the drivebase. To be used before auto starts + */ + public void resetRobot() { + swerveDrive.resetEncoders(); + resetGyro(); + setPose(new Pose2d()); + } +} diff --git a/src/main/java/frc/team2412/robot/util/MACAddress.java b/src/main/java/frc/team2412/robot/util/MACAddress.java new file mode 100644 index 00000000..8b3fc26c --- /dev/null +++ b/src/main/java/frc/team2412/robot/util/MACAddress.java @@ -0,0 +1,98 @@ +package frc.team2412.robot.util; + +import java.io.IOException; +import java.net.NetworkInterface; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.Enumeration; +import java.util.List; + +/** + * Class to deal with robot MAC Addresses + * + * @author Alex Stedman + */ +@SuppressWarnings("EqualsBetweenInconvertibleTypes") +public class MACAddress { + private final byte[] address; + + /** + * Create MAC Address + * + * @param addresses bytes of address + */ + public MACAddress(byte... addresses) { + address = addresses; + } + + /** + * get the bytes of the address + * + * @return the bytes of the address + */ + public byte[] getAddress() { + return address; + } + + /** + * does this MAC Address equal another or do the bytes match? + * + * @param obj thing to compare + * @return if they should be the same + */ + public boolean matches(Object obj) { + if (obj instanceof byte[]) return Arrays.equals(address, (byte[]) obj); + if (obj instanceof MACAddress) return matches(((MACAddress) obj).getAddress()); + return false; + } + + /** + * does this mac address exist on the network + * + * @return if this mac address exists on the network + */ + public boolean exists() { + try { + for (byte[] b : getAll()) { + if (matches(b)) return true; + } + return false; + } catch (IOException ignored) { + return false; + } + } + + /** + * get all mac addresses on the network + * + * @return a List of the byte addresses + * @throws IOException if network cannot be found + */ + public static List getAll() throws IOException { + List macAddresses = new ArrayList<>(); + + Enumeration networkInterfaces = NetworkInterface.getNetworkInterfaces(); + + NetworkInterface networkInterface; + while (networkInterfaces.hasMoreElements()) { + networkInterface = networkInterfaces.nextElement(); + + byte[] address = networkInterface.getHardwareAddress(); + if (address != null) macAddresses.add(address); + } + + return macAddresses; + } + + /** + * Shorthand create a simplified addres + * + * @param a 4th byte term as int + * @param b 5th byte term as int + * @param c 6th byte term as int + * @return new MACAddress + */ + public static MACAddress of(int a, int b, int c) { + return new MACAddress((byte) 0x00, (byte) 0x80, (byte) 0x2f, (byte) a, (byte) b, (byte) c); + } +} diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json new file mode 100644 index 00000000..3e18a45d --- /dev/null +++ b/vendordeps/PathplannerLib.json @@ -0,0 +1,38 @@ +{ + "fileName": "PathplannerLib.json", + "name": "PathplannerLib", + "version": "2024.0.0-beta-6.2", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2024", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2024.0.0-beta-6.2" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2024.0.0-beta-6.2", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json new file mode 100644 index 00000000..ebbff7d2 --- /dev/null +++ b/vendordeps/Phoenix6.json @@ -0,0 +1,339 @@ +{ + "fileName": "Phoenix6.json", + "name": "CTRE-Phoenix (v6)", + "version": "24.0.0-beta-7", + "frcYear": 2024, + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-beta-latest.json", + "conflictsWith": [ + { + "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", + "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", + "offlineFileName": "Phoenix6And5.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "24.0.0-beta-7" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "24.0.0-beta-7", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "24.0.0-beta-7", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "24.0.0-beta-7", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonFX", + "version": "24.0.0-beta-7", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "24.0.0-beta-7", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "24.0.0-beta-7", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "24.0.0-beta-7", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "24.0.0-beta-7", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "24.0.0-beta-7", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "24.0.0-beta-7", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "24.0.0-beta-7", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "24.0.0-beta-7", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "24.0.0-beta-7", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "24.0.0-beta-7", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "24.0.0-beta-7", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonFX", + "version": "24.0.0-beta-7", + "libName": "CTRE_SimTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "24.0.0-beta-7", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "24.0.0-beta-7", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "24.0.0-beta-7", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "24.0.0-beta-7", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "24.0.0-beta-7", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "24.0.0-beta-7", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/yagsl.json b/vendordeps/yagsl.json new file mode 100644 index 00000000..b5ece1a5 --- /dev/null +++ b/vendordeps/yagsl.json @@ -0,0 +1,19 @@ +{ + "fileName": "yagsl.json", + "name": "YAGSL", + "version": "2023.2.9", + "uuid": "1ccce5a4-acd2-4d18-bca3-4b8047188400", + "mavenUrls": [ + "https://broncbotz3481.github.io/YAGSL-Lib/yagsl/repos" + ], + "jsonUrl": "https://broncbotz3481.github.io/YAGSL-Lib/yagsl/yagsl.json", + "javaDependencies": [ + { + "groupId": "swervelib", + "artifactId": "YAGSL-java", + "version": "2023.2.9" + } + ], + "jniDependencies": [], + "cppDependencies": [] +} \ No newline at end of file From f4a3daa265c65d8dba81068d10d2e454c4961d93 Mon Sep 17 00:00:00 2001 From: DriverStation3 Date: Sat, 6 Jan 2024 14:41:11 -0800 Subject: [PATCH 02/12] add drive commands, finish up drivesubsystem --- .../java/frc/team2412/robot/Controls.java | 22 ++- src/main/java/frc/team2412/robot/Robot.java | 4 +- .../java/frc/team2412/robot/Subsystems.java | 11 +- .../robot/subsystems/DrivebaseSubsystem.java | 126 ++++++++++-------- 4 files changed, 106 insertions(+), 57 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index ff619646..a52c9bff 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -1,7 +1,12 @@ package frc.team2412.robot; -import static frc.team2412.robot.Controls.ControlConstants.*; +import static frc.team2412.robot.Controls.ControlConstants.CODRIVER_CONTROLLER_PORT; +import static frc.team2412.robot.Controls.ControlConstants.CONTROLLER_PORT; +import static frc.team2412.robot.Subsystems.SubsystemConstants.DRIVEBASE_ENABLED; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; public class Controls { @@ -19,5 +24,20 @@ public Controls(Subsystems s) { driveController = new CommandXboxController(CONTROLLER_PORT); codriveController = new CommandXboxController(CODRIVER_CONTROLLER_PORT); this.s = s; + + if (DRIVEBASE_ENABLED) { + bindDrivebaseControls(); + } + } + + private void bindDrivebaseControls() { + CommandScheduler.getInstance() + .setDefaultCommand( + s.drivebaseSubsystem, + s.drivebaseSubsystem.driveJoystick( + driveController::getLeftY, + driveController::getLeftX, + () -> Rotation2d.fromRotations(driveController.getRightX()))); + driveController.start().onTrue(new InstantCommand(s.drivebaseSubsystem::resetGyro)); } } diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index 7ef07987..ca4a9d7f 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -43,7 +43,9 @@ protected Robot() { private static RobotType getTypeFromAddress() { if (PRACTICE_ADDRESS.exists()) return RobotType.PRACTICE; - else {return RobotType.COMPETITION;} + else { + return RobotType.COMPETITION; + } } @Override diff --git a/src/main/java/frc/team2412/robot/Subsystems.java b/src/main/java/frc/team2412/robot/Subsystems.java index 260e6765..ab8ff6ba 100644 --- a/src/main/java/frc/team2412/robot/Subsystems.java +++ b/src/main/java/frc/team2412/robot/Subsystems.java @@ -1,12 +1,21 @@ package frc.team2412.robot; +import static frc.team2412.robot.Subsystems.SubsystemConstants.*; + +import frc.team2412.robot.subsystems.DrivebaseSubsystem; + public class Subsystems { public static class SubsystemConstants { - public static final boolean DRIVEBASE_ENABLED = false; + private static final boolean IS_COMP = Robot.getInstance().isCompetition(); + public static final boolean DRIVEBASE_ENABLED = true; } + public final DrivebaseSubsystem drivebaseSubsystem; public Subsystems() { // initialize subsystems here (wow thats wild) + if (DRIVEBASE_ENABLED) { + drivebaseSubsystem = new DrivebaseSubsystem(); + } } } diff --git a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java index fce7eafd..7b6793d2 100644 --- a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java @@ -1,79 +1,97 @@ package frc.team2412.robot.subsystems; -import java.io.File; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.team2412.robot.Robot; +import java.io.File; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; import swervelib.SwerveDrive; +import swervelib.math.SwerveMath; import swervelib.parser.SwerveParser; public class DrivebaseSubsystem extends SubsystemBase { - // SWERVE CONSTANTS (that aren't in deploy dir) + // SWERVE CONSTANTS (that aren't in deploy dir) + + private static final double MAX_SPEED = 1.0; + private static final double JOYSTICK_DEADBAND = 0.05; - private static final double MAX_SPEED = 1.0; + private final SwerveDrive swerveDrive; - private final SwerveDrive swerveDrive; + public DrivebaseSubsystem() { + File swerveJsonDirectory; - public DrivebaseSubsystem() { - File swerveJsonDirectory; + if (Robot.getInstance().isCompetition()) { + swerveJsonDirectory = new File(Filesystem.getDeployDirectory(), "swerve"); + } else { + swerveJsonDirectory = new File(Filesystem.getDeployDirectory(), "practiceswerve"); + } - if (Robot.getInstance().isCompetition()) { - swerveJsonDirectory = new File(Filesystem.getDeployDirectory(), "swerve"); - } else { - swerveJsonDirectory = new File(Filesystem.getDeployDirectory(), "practiceswerve"); - } + try { + swerveDrive = new SwerveParser(swerveJsonDirectory).createSwerveDrive(MAX_SPEED); + } catch (Exception e) { + throw new RuntimeException(); + } - try { - swerveDrive = new SwerveParser(swerveJsonDirectory).createSwerveDrive(MAX_SPEED); - } catch (Exception e) { - throw new RuntimeException(); - } + // set drive motors to brake + swerveDrive.setMotorIdleMode(true); + // enable optimization (never move the angle wheels more than 90 degrees) + swerveDrive.setModuleStateOptimization(true); + // swerve drive heading will slowly drift over time as you translate. this method enables an + // active correction using pid. disabled until testing can be done + swerveDrive.setHeadingCorrection(false); + // supposed to do something? see + // https://broncbotz3481.github.io/YAGSL/swervelib/SwerveDrive.html#chassisVelocityCorrection + swerveDrive.chassisVelocityCorrection = true; - // set drive motors to brake - swerveDrive.setMotorIdleMode(true); - // enable optimization (never move the angle wheels more than 90 degrees) - swerveDrive.setModuleStateOptimization(true); - // swerve drive heading will slowly drift over time as you translate. this method enables an active correction using pid. disabled until testing can be done - swerveDrive.setHeadingCorrection(false); - // supposed to do something? see https://broncbotz3481.github.io/YAGSL/swervelib/SwerveDrive.html#chassisVelocityCorrection - swerveDrive.chassisVelocityCorrection = true; + swerveDrive.synchronizeModuleEncoders(); + } - swerveDrive.synchronizeModuleEncoders(); - } + public void drive(Translation2d translation, Rotation2d rotation, boolean fieldOriented) { + swerveDrive.drive(translation, rotation.getRadians(), fieldOriented, false); + } - public void drive( - Translation2d translation, - Rotation2d rotation, - boolean fieldOriented) { - swerveDrive.drive(translation, rotation.getRadians(), fieldOriented, false); - } + /** + * Drives the robot using joystick inputs + * + * @param forward Forward motion in meters. A negative value makes the robot go backwards + * @param strafe Strafe motion in meters. A negative value makes the robot go left + * @param rotation Rotation2d value of robot rotation. CW is positive TODO: is this true? + */ + public Command driveJoystick( + DoubleSupplier forward, DoubleSupplier strafe, Supplier rotation) { + Rotation2d constrainedRotation = + Rotation2d.fromRotations( + SwerveMath.applyDeadband(rotation.get().getRotations(), true, JOYSTICK_DEADBAND)); + Translation2d constrainedTranslation = + new Translation2d( + SwerveMath.applyDeadband(forward.getAsDouble(), true, JOYSTICK_DEADBAND), + SwerveMath.applyDeadband(strafe.getAsDouble(), true, JOYSTICK_DEADBAND)); + return this.run(() -> drive(constrainedTranslation, constrainedRotation, true)); + } - /* - * Set the robot's pose. - * TODO: does this change yaw too? does this affect field oriented? - */ - public void setPose(Pose2d pose) { - swerveDrive.resetOdometry(pose); - } + /** Set the robot's pose. TODO: does this change yaw too? does this affect field oriented? */ + public void setPose(Pose2d pose) { + swerveDrive.resetOdometry(pose); + } - /* - * Reset the gyro angle. After this method is called, yaw will be zero. Pose is also updated to zero rotation but same position - */ - public void resetGyro() { - swerveDrive.zeroGyro(); - } + /** + * Reset the gyro angle. After this method is called, yaw will be zero. Pose is also updated to + * zero rotation but same position + */ + public void resetGyro() { + swerveDrive.zeroGyro(); + } - /* - * Reset everything we can on the drivebase. To be used before auto starts - */ - public void resetRobot() { - swerveDrive.resetEncoders(); - resetGyro(); - setPose(new Pose2d()); - } + /** Reset everything we can on the drivebase. To be used before auto starts */ + public void resetRobot() { + swerveDrive.resetEncoders(); + resetGyro(); + setPose(new Pose2d()); + } } From 98bc57dd0e92a66cedba595c80aebeae573ef48b Mon Sep 17 00:00:00 2001 From: DriverStation3 Date: Sat, 6 Jan 2024 15:58:33 -0800 Subject: [PATCH 03/12] get a start to pathplanner --- .../robot/subsystems/DrivebaseSubsystem.java | 51 +++++++++++++++++++ 1 file changed, 51 insertions(+) diff --git a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java index 7b6793d2..7ffabff4 100644 --- a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java @@ -1,8 +1,13 @@ package frc.team2412.robot.subsystems; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.PIDConstants; +import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.geometry.Pose2d; 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.wpilibj.Filesystem; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -20,6 +25,13 @@ public class DrivebaseSubsystem extends SubsystemBase { private static final double MAX_SPEED = 1.0; private static final double JOYSTICK_DEADBAND = 0.05; + private static final double DRIVEBASE_RADIUS = 0; + + // AUTO CONSTANTS + + private static final PIDConstants AUTO_TRANSLATION_PID = new PIDConstants(0, 0, 0); + private static final PIDConstants AUTO_ROTATION_PID = new PIDConstants(0, 0, 0); + private static final double MAX_AUTO_SPEED = 0; private final SwerveDrive swerveDrive; @@ -50,8 +62,38 @@ public DrivebaseSubsystem() { swerveDrive.chassisVelocityCorrection = true; swerveDrive.synchronizeModuleEncoders(); + + // Configure auto builder for PathPlanner + AutoBuilder.configureHolonomic( + this::getPose, + this::setPose, + this::getRobotSpeeds, + this::drive, + new HolonomicPathFollowerConfig( + AUTO_TRANSLATION_PID, + AUTO_ROTATION_PID, + MAX_AUTO_SPEED, + DRIVEBASE_RADIUS, + new ReplanningConfig()), + this); + } + + /** + * Drive with robot-relative chassis speeds + * + * @param speeds Robot-relative speeds + */ + public void drive(ChassisSpeeds speeds) { + swerveDrive.drive(speeds); } + /** + * Drive the robot + * + * @param translation + * @param rotation + * @param fieldOriented Whether these values are field oriented + */ public void drive(Translation2d translation, Rotation2d rotation, boolean fieldOriented) { swerveDrive.drive(translation, rotation.getRadians(), fieldOriented, false); } @@ -75,11 +117,20 @@ public Command driveJoystick( return this.run(() -> drive(constrainedTranslation, constrainedRotation, true)); } + public ChassisSpeeds getRobotSpeeds() { + return swerveDrive.getRobotVelocity(); + } + /** Set the robot's pose. TODO: does this change yaw too? does this affect field oriented? */ public void setPose(Pose2d pose) { swerveDrive.resetOdometry(pose); } + /** Get the robot's pose */ + public Pose2d getPose() { + return swerveDrive.getPose(); + } + /** * Reset the gyro angle. After this method is called, yaw will be zero. Pose is also updated to * zero rotation but same position From 3810d7a0f5df8fc2754659c919a549cd3b239b70 Mon Sep 17 00:00:00 2001 From: DriverStation3 Date: Mon, 8 Jan 2024 18:59:33 -0800 Subject: [PATCH 04/12] start updating to 2024 wpilib --- .editorconfig | 11 - .gitattributes | 1 - .github/workflows/check.yml | 25 - .gitignore | 32 +- .vscode/extensions.json | 16 - .vscode/launch.json | 21 + .vscode/settings.json | 28 +- .wpilib/wpilib_preferences.json | 10 +- LICENSE-APACHE | 201 --- LICENSE-MIT | 21 - README.md | 18 - WPILib-License.md | 18 +- build.gradle | 154 +-- gradle.properties | 6 - gradle/wrapper/gradle-wrapper.jar | Bin 59536 -> 43462 bytes gradle/wrapper/gradle-wrapper.properties | 4 +- gradlew | 190 +-- gradlew.bat | 15 +- gradlew.ps1 | 76 -- settings.gradle | 53 +- .../robot/subsystems/DrivebaseSubsystem.java | 1 + src/main/java/swervelib/SwerveController.java | 234 ++++ src/main/java/swervelib/SwerveDrive.java | 1082 +++++++++++++++++ src/main/java/swervelib/SwerveModule.java | 437 +++++++ .../encoders/AnalogAbsoluteEncoderSwerve.java | 120 ++ .../swervelib/encoders/CANCoderSwerve.java | 171 +++ .../swervelib/encoders/CanAndCoderSwerve.java | 107 ++ .../encoders/PWMDutyCycleEncoderSwerve.java | 112 ++ .../encoders/SparkMaxAnalogEncoderSwerve.java | 130 ++ .../encoders/SparkMaxEncoderSwerve.java | 141 +++ .../encoders/SwerveAbsoluteEncoder.java | 62 + .../java/swervelib/encoders/package-info.java | 4 + .../java/swervelib/imu/ADIS16448Swerve.java | 108 ++ .../java/swervelib/imu/ADIS16470Swerve.java | 107 ++ .../java/swervelib/imu/ADXRS450Swerve.java | 106 ++ .../java/swervelib/imu/AnalogGyroSwerve.java | 113 ++ src/main/java/swervelib/imu/NavXSwerve.java | 174 +++ .../java/swervelib/imu/Pigeon2Swerve.java | 136 +++ src/main/java/swervelib/imu/PigeonSwerve.java | 113 ++ src/main/java/swervelib/imu/SwerveIMU.java | 58 + src/main/java/swervelib/imu/package-info.java | 4 + src/main/java/swervelib/math/Matter.java | 41 + src/main/java/swervelib/math/SwerveMath.java | 402 ++++++ .../java/swervelib/math/package-info.java | 6 + .../motors/SparkMaxBrushedMotorSwerve.java | 418 +++++++ .../java/swervelib/motors/SparkMaxSwerve.java | 441 +++++++ .../java/swervelib/motors/SwerveMotor.java | 160 +++ .../java/swervelib/motors/TalonFXSwerve.java | 428 +++++++ .../java/swervelib/motors/TalonSRXSwerve.java | 415 +++++++ .../java/swervelib/motors/package-info.java | 4 + src/main/java/swervelib/package-info.java | 6 + .../java/swervelib/parser/PIDFConfig.java | 108 ++ .../parser/SwerveControllerConfiguration.java | 63 + .../parser/SwerveDriveConfiguration.java | 95 ++ .../parser/SwerveModuleConfiguration.java | 162 +++ .../SwerveModulePhysicalCharacteristics.java | 106 ++ .../java/swervelib/parser/SwerveParser.java | 227 ++++ .../parser/deserializer/PIDFRange.java | 17 + .../parser/deserializer/package-info.java | 4 + .../parser/json/ControllerPropertiesJson.java | 35 + .../swervelib/parser/json/DeviceJson.java | 187 +++ .../swervelib/parser/json/ModuleJson.java | 137 +++ .../parser/json/MotorConfigDouble.java | 36 + .../swervelib/parser/json/MotorConfigInt.java | 36 + .../parser/json/PIDFPropertiesJson.java | 19 + .../parser/json/PhysicalPropertiesJson.java | 52 + .../parser/json/SwerveDriveJson.java | 21 + .../parser/json/modules/BoolMotorJson.java | 17 + .../parser/json/modules/LocationJson.java | 18 + .../parser/json/modules/package-info.java | 4 + .../swervelib/parser/json/package-info.java | 4 + .../java/swervelib/parser/package-info.java | 4 + .../simulation/SwerveIMUSimulation.java | 122 ++ .../simulation/SwerveModuleSimulation.java | 91 ++ .../swervelib/simulation/package-info.java | 4 + .../telemetry/SwerveDriveTelemetry.java | 118 ++ .../swervelib/telemetry/package-info.java | 4 + vendordeps/PathplannerLib.json | 38 - vendordeps/Phoenix6.json | 339 ------ vendordeps/WPILibNewCommands.json | 73 +- vendordeps/yagsl.json | 19 - 81 files changed, 7844 insertions(+), 1057 deletions(-) delete mode 100644 .editorconfig delete mode 100644 .gitattributes delete mode 100644 .github/workflows/check.yml delete mode 100644 .vscode/extensions.json create mode 100644 .vscode/launch.json delete mode 100644 LICENSE-APACHE delete mode 100644 LICENSE-MIT delete mode 100644 README.md delete mode 100644 gradle.properties delete mode 100644 gradlew.ps1 create mode 100644 src/main/java/swervelib/SwerveController.java create mode 100644 src/main/java/swervelib/SwerveDrive.java create mode 100644 src/main/java/swervelib/SwerveModule.java create mode 100644 src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java create mode 100644 src/main/java/swervelib/encoders/CANCoderSwerve.java create mode 100644 src/main/java/swervelib/encoders/CanAndCoderSwerve.java create mode 100644 src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java create mode 100644 src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java create mode 100644 src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java create mode 100644 src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java create mode 100644 src/main/java/swervelib/encoders/package-info.java create mode 100644 src/main/java/swervelib/imu/ADIS16448Swerve.java create mode 100644 src/main/java/swervelib/imu/ADIS16470Swerve.java create mode 100644 src/main/java/swervelib/imu/ADXRS450Swerve.java create mode 100644 src/main/java/swervelib/imu/AnalogGyroSwerve.java create mode 100644 src/main/java/swervelib/imu/NavXSwerve.java create mode 100644 src/main/java/swervelib/imu/Pigeon2Swerve.java create mode 100644 src/main/java/swervelib/imu/PigeonSwerve.java create mode 100644 src/main/java/swervelib/imu/SwerveIMU.java create mode 100644 src/main/java/swervelib/imu/package-info.java create mode 100644 src/main/java/swervelib/math/Matter.java create mode 100644 src/main/java/swervelib/math/SwerveMath.java create mode 100644 src/main/java/swervelib/math/package-info.java create mode 100644 src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java create mode 100644 src/main/java/swervelib/motors/SparkMaxSwerve.java create mode 100644 src/main/java/swervelib/motors/SwerveMotor.java create mode 100644 src/main/java/swervelib/motors/TalonFXSwerve.java create mode 100644 src/main/java/swervelib/motors/TalonSRXSwerve.java create mode 100644 src/main/java/swervelib/motors/package-info.java create mode 100644 src/main/java/swervelib/package-info.java create mode 100644 src/main/java/swervelib/parser/PIDFConfig.java create mode 100644 src/main/java/swervelib/parser/SwerveControllerConfiguration.java create mode 100644 src/main/java/swervelib/parser/SwerveDriveConfiguration.java create mode 100644 src/main/java/swervelib/parser/SwerveModuleConfiguration.java create mode 100644 src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java create mode 100644 src/main/java/swervelib/parser/SwerveParser.java create mode 100644 src/main/java/swervelib/parser/deserializer/PIDFRange.java create mode 100644 src/main/java/swervelib/parser/deserializer/package-info.java create mode 100644 src/main/java/swervelib/parser/json/ControllerPropertiesJson.java create mode 100644 src/main/java/swervelib/parser/json/DeviceJson.java create mode 100644 src/main/java/swervelib/parser/json/ModuleJson.java create mode 100644 src/main/java/swervelib/parser/json/MotorConfigDouble.java create mode 100644 src/main/java/swervelib/parser/json/MotorConfigInt.java create mode 100644 src/main/java/swervelib/parser/json/PIDFPropertiesJson.java create mode 100644 src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java create mode 100644 src/main/java/swervelib/parser/json/SwerveDriveJson.java create mode 100644 src/main/java/swervelib/parser/json/modules/BoolMotorJson.java create mode 100644 src/main/java/swervelib/parser/json/modules/LocationJson.java create mode 100644 src/main/java/swervelib/parser/json/modules/package-info.java create mode 100644 src/main/java/swervelib/parser/json/package-info.java create mode 100644 src/main/java/swervelib/parser/package-info.java create mode 100644 src/main/java/swervelib/simulation/SwerveIMUSimulation.java create mode 100644 src/main/java/swervelib/simulation/SwerveModuleSimulation.java create mode 100644 src/main/java/swervelib/simulation/package-info.java create mode 100644 src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java create mode 100644 src/main/java/swervelib/telemetry/package-info.java delete mode 100644 vendordeps/PathplannerLib.json delete mode 100644 vendordeps/Phoenix6.json delete mode 100644 vendordeps/yagsl.json diff --git a/.editorconfig b/.editorconfig deleted file mode 100644 index 07dc5352..00000000 --- a/.editorconfig +++ /dev/null @@ -1,11 +0,0 @@ -root = true - -[*] -charset = utf-8 -indent_style = tab -end_of_line = lf -insert_final_newline = true -trim_trailing_whitespace = true - -[*.yml] -indent_style = space diff --git a/.gitattributes b/.gitattributes deleted file mode 100644 index 94f480de..00000000 --- a/.gitattributes +++ /dev/null @@ -1 +0,0 @@ -* text=auto eol=lf \ No newline at end of file diff --git a/.github/workflows/check.yml b/.github/workflows/check.yml deleted file mode 100644 index 7903052f..00000000 --- a/.github/workflows/check.yml +++ /dev/null @@ -1,25 +0,0 @@ -name: Gradle Check -on: - pull_request: - branches: [main] - push: - branches: [main] - -jobs: - check: - runs-on: ubuntu-latest - - steps: - - uses: actions/checkout@v3 - - - uses: actions/setup-java@v3 - with: - distribution: temurin - java-version: 11 - cache: gradle - - - name: Check - run: ./gradlew check - - - name: Automated Sim Test - run: ./gradlew simulateJavaDebug -PautomatedTest=true diff --git a/.gitignore b/.gitignore index 13cedb39..5528d4f6 100644 --- a/.gitignore +++ b/.gitignore @@ -1,8 +1,6 @@ # This gitignore has been specially created by the WPILib team. # If you remove items from this file, intellisense might break. -.Glass - ### C++ ### # Prerequisites *.d @@ -106,13 +104,11 @@ Temporary Items .apdisk ### VisualStudioCode ### -.factorypath .vscode/* !.vscode/settings.json !.vscode/tasks.json !.vscode/launch.json !.vscode/extensions.json -.factorypath ### Windows ### # Windows thumbnail cache files @@ -162,21 +158,21 @@ gradle-app.setting .settings/ bin/ -# Simulation GUI and other tools window save file -*-window.json -simgui*.json -.wpilog +# IntelliJ +*.iml +*.ipr +*.iws +.idea/ +out/ -# Vim :) -Session.vim -.undodir +# Fleet +.fleet -# Intellij IDEA platform -.idea -local.properties +# Simulation GUI and other tools window save file +*-window.json -# WPILib datalog tool config files -.DataLogTool/ +# Simulation data log directory +logs/ -# Don't know what's creating this -.project +# Folder that has CTRE Phoenix Sim device config storage +ctre_sim/ diff --git a/.vscode/extensions.json b/.vscode/extensions.json deleted file mode 100644 index 2d30ac4d..00000000 --- a/.vscode/extensions.json +++ /dev/null @@ -1,16 +0,0 @@ -{ - "recommendations": [ - "editorconfig.editorconfig", - "vscjava.vscode-java-pack", - "github.vscode-pull-request-github", - "vscjava.vscode-gradle", - "gruntfuggly.todo-tree", - "wpilibsuite.vscode-wpilib", - "redhat.java", - "vscjava.vscode-java-debug", - "vscjava.vscode-java-test", - "vscjava.vscode-maven", - "vscjava.vscode-java-dependency", - "visualstudioexptteam.vscodeintellicode" - ] -} diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 00000000..c9c9713d --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,21 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + + { + "type": "wpilib", + "name": "WPILib Desktop Debug", + "request": "launch", + "desktop": true, + }, + { + "type": "wpilib", + "name": "WPILib roboRIO Debug", + "request": "launch", + "desktop": false, + } + ] +} diff --git a/.vscode/settings.json b/.vscode/settings.json index 4cd8c4fe..4ed293b7 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,3 +1,29 @@ { - "java.configuration.updateBuildConfiguration": "automatic" + "java.configuration.updateBuildConfiguration": "automatic", + "java.server.launchMode": "Standard", + "files.exclude": { + "**/.git": true, + "**/.svn": true, + "**/.hg": true, + "**/CVS": true, + "**/.DS_Store": true, + "bin/": true, + "**/.classpath": true, + "**/.project": true, + "**/.settings": true, + "**/.factorypath": true, + "**/*~": true + }, + "java.test.config": [ + { + "name": "WPIlibUnitTests", + "workingDirectory": "${workspaceFolder}/build/jni/release", + "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ], + "env": { + "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" , + "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" + } + }, + ], + "java.test.defaultConfig": "WPIlibUnitTests" } diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 2db13a76..b7bcd22d 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { - "enableCppIntellisense": false, - "currentLanguage": "java", - "projectYear": "2024", - "teamNumber": 2412 -} + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2024", + "teamNumber": 2412 +} \ No newline at end of file diff --git a/LICENSE-APACHE b/LICENSE-APACHE deleted file mode 100644 index f4bf037e..00000000 --- a/LICENSE-APACHE +++ /dev/null @@ -1,201 +0,0 @@ - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - -TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - -1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - -2. Grant of Copyright License. Subject to the terms and conditions of -this License, each Contributor hereby grants to You a perpetual, -worldwide, non-exclusive, no-charge, royalty-free, irrevocable -copyright license to reproduce, prepare Derivative Works of, -publicly display, publicly perform, sublicense, and distribute the -Work and such Derivative Works in Source or Object form. - -3. Grant of Patent License. Subject to the terms and conditions of -this License, each Contributor hereby grants to You a perpetual, -worldwide, non-exclusive, no-charge, royalty-free, irrevocable -(except as stated in this section) patent license to make, have made, -use, offer to sell, sell, import, and otherwise transfer the Work, -where such license applies only to those patent claims licensable -by such Contributor that are necessarily infringed by their -Contribution(s) alone or by combination of their Contribution(s) -with the Work to which such Contribution(s) was submitted. If You -institute patent litigation against any entity (including a -cross-claim or counterclaim in a lawsuit) alleging that the Work -or a Contribution incorporated within the Work constitutes direct -or contributory patent infringement, then any patent licenses -granted to You under this License for that Work shall terminate -as of the date such litigation is filed. - -4. Redistribution. You may reproduce and distribute copies of the -Work or Derivative Works thereof in any medium, with or without -modifications, and in Source or Object form, provided that You -meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - -5. Submission of Contributions. Unless You explicitly state otherwise, -any Contribution intentionally submitted for inclusion in the Work -by You to the Licensor shall be under the terms and conditions of -this License, without any additional terms or conditions. -Notwithstanding the above, nothing herein shall supersede or modify -the terms of any separate license agreement you may have executed -with Licensor regarding such Contributions. - -6. Trademarks. This License does not grant permission to use the trade -names, trademarks, service marks, or product names of the Licensor, -except as required for reasonable and customary use in describing the -origin of the Work and reproducing the content of the NOTICE file. - -7. Disclaimer of Warranty. Unless required by applicable law or -agreed to in writing, Licensor provides the Work (and each -Contributor provides its Contributions) on an "AS IS" BASIS, -WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or -implied, including, without limitation, any warranties or conditions -of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A -PARTICULAR PURPOSE. You are solely responsible for determining the -appropriateness of using or redistributing the Work and assume any -risks associated with Your exercise of permissions under this License. - -8. Limitation of Liability. In no event and under no legal theory, -whether in tort (including negligence), contract, or otherwise, -unless required by applicable law (such as deliberate and grossly -negligent acts) or agreed to in writing, shall any Contributor be -liable to You for damages, including any direct, indirect, special, -incidental, or consequential damages of any character arising as a -result of this License or out of the use or inability to use the -Work (including but not limited to damages for loss of goodwill, -work stoppage, computer failure or malfunction, or any and all -other commercial damages or losses), even if such Contributor -has been advised of the possibility of such damages. - -9. Accepting Warranty or Additional Liability. While redistributing -the Work or Derivative Works thereof, You may choose to offer, -and charge a fee for, acceptance of support, warranty, indemnity, -or other liability obligations and/or rights consistent with this -License. However, in accepting such obligations, You may act only -on Your own behalf and on Your sole responsibility, not on behalf -of any other Contributor, and only if You agree to indemnify, -defend, and hold each Contributor harmless for any liability -incurred by, or claims asserted against, such Contributor by reason -of your accepting any such warranty or additional liability. - -END OF TERMS AND CONDITIONS - -APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - -Copyright [yyyy][name of copyright owner] - -Licensed under the Apache License, Version 2.0 (the "License"); -you may not use this file except in compliance with the License. -You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - -Unless required by applicable law or agreed to in writing, software -distributed under the License is distributed on an "AS IS" BASIS, -WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -See the License for the specific language governing permissions and -limitations under the License. diff --git a/LICENSE-MIT b/LICENSE-MIT deleted file mode 100644 index 8aa26455..00000000 --- a/LICENSE-MIT +++ /dev/null @@ -1,21 +0,0 @@ -MIT License - -Copyright (c) [year] [fullname] - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. diff --git a/README.md b/README.md deleted file mode 100644 index adeaa1fa..00000000 --- a/README.md +++ /dev/null @@ -1,18 +0,0 @@ -### License - -Licensed under either the - -- Apache License, Version 2.0, ([LICENSE-APACHE](LICENSE-APACHE) or http://www.apache.org/licenses/LICENSE-2.0) -- MIT license ([LICENSE-MIT](LICENSE-MIT) or http://opensource.org/licenses/MIT) - -at your option. - -### Contribution - -Unless you explicitly state otherwise, any contribution intentionally submitted -for inclusion in the work by you, as defined in the Apache-2.0 license, shall be dual licensed as above, without any -additional terms or conditions. - -### Credits - -Thanks to [Bevy](https://github.com/bevyengine/bevy/issues/2373) for the License and Contribution sections in the README. diff --git a/WPILib-License.md b/WPILib-License.md index 4b5cc11f..43b62ec2 100644 --- a/WPILib-License.md +++ b/WPILib-License.md @@ -1,16 +1,16 @@ -Copyright (c) 2009-2021 FIRST and other WPILib contributors +Copyright (c) 2009-2023 FIRST and other WPILib contributors All rights reserved. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -* Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. -* Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. -* Neither the name of FIRST, WPILib, nor the names of other WPILib - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of FIRST, WPILib, nor the names of other WPILib + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED diff --git a/build.gradle b/build.gradle index 479ffb42..a8ca9c5b 100644 --- a/build.gradle +++ b/build.gradle @@ -1,56 +1,42 @@ plugins { - id "java" - id "com.diffplug.spotless" version "6.15.0" - id "edu.wpi.first.GradleRIO" version "2023.4.3" - id "net.ltgt.errorprone" + id "java" + id "edu.wpi.first.GradleRIO" version "2024.1.1" } -repositories { - mavenCentral() - maven { url "https://jitpack.io" } +java { + sourceCompatibility = JavaVersion.VERSION_17 + targetCompatibility = JavaVersion.VERSION_17 } -sourceCompatibility = JavaVersion.VERSION_11 -targetCompatibility = JavaVersion.VERSION_11 - -def getProjectBooleanProperty(String name, boolean defaultValue = false) { - if (!project.hasProperty(name)) { - return defaultValue - } - // Note that this is equivalent to 'true'.equalsIgnoreCase(project.getProperty(name)) - // Values that are not "true" or "false" (including null) are treated as false - return Boolean.parseBoolean(project.getProperty(name)) -} - -def ROBOT_MAIN_CLASS = getProjectBooleanProperty('automatedTest') ? "frc.team2412.robot.test.AutomatedTestMain" : "frc.team2412.robot.Main" +def ROBOT_MAIN_CLASS = "" // Define my targets (RoboRIO) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. deploy { - targets { - roborio(getTargetTypeClass("RoboRIO")) { - // Team number is loaded either from the .wpilib/wpilib_preferences.json - // or from command line. If not found an exception will be thrown. - // You can use getTeamOrDefault(team) instead of getTeamNumber if you - // want to store a team number in this file. - team = project.frc.getTeamNumber() - debug = project.frc.getDebugOrDefault(false) - - artifacts { - // First part is artifact name, 2nd is artifact type - // getTargetTypeClass is a shortcut to get the class type using a string - - frcJava(getArtifactTypeClass("FRCJavaArtifact")) { - } - - // Static files artifact - frcStaticFileDeploy(getArtifactTypeClass("FileTreeArtifact")) { - files = project.fileTree("src/main/deploy") - directory = "/home/lvuser/deploy" - } - } - } - } + targets { + roborio(getTargetTypeClass('RoboRIO')) { + // Team number is loaded either from the .wpilib/wpilib_preferences.json + // or from command line. If not found an exception will be thrown. + // You can use getTeamOrDefault(team) instead of getTeamNumber if you + // want to store a team number in this file. + team = project.frc.getTeamNumber() + debug = project.frc.getDebugOrDefault(false) + + artifacts { + // First part is artifact name, 2nd is artifact type + // getTargetTypeClass is a shortcut to get the class type using a string + + frcJava(getArtifactTypeClass('FRCJavaArtifact')) { + } + + // Static files artifact + frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { + files = project.fileTree('src/main/deploy') + directory = '/home/lvuser/deploy' + } + } + } + } } def deployArtifact = deploy.targets.roborio.artifacts.frcJava @@ -62,51 +48,46 @@ wpi.java.debugJni = false def includeDesktopSupport = true // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. -// Also defines JUnit 4. +// Also defines JUnit 5. dependencies { - implementation 'org.jetbrains:annotations:19.0.0' - compileOnly group: "com.google.errorprone", name: "error_prone_annotations", version: "2.10.0" - - implementation wpi.java.deps.wpilib() - implementation wpi.java.vendor.java() + implementation wpi.java.deps.wpilib() + implementation wpi.java.vendor.java() - roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) - roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) + roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) + roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) - roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio) - roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio) + roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio) + roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio) - nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) - nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) - simulationDebug wpi.sim.enableDebug() + nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) + nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) + simulationDebug wpi.sim.enableDebug() - nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop) - nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) - simulationRelease wpi.sim.enableRelease() + nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop) + nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) + simulationRelease wpi.sim.enableRelease() - testImplementation "junit:junit:4.12" - - errorprone group: "com.google.errorprone", name: "error_prone_core", version: "2.10.0" + testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' } -tasks.withType(JavaCompile).configureEach { - options.errorprone.disableWarningsInGeneratedCode = true +test { + useJUnitPlatform() + systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' } // Simulation configuration (e.g. environment variables). +wpi.sim.addGui().defaultEnabled = true +wpi.sim.addDriverstation() -if (!getProjectBooleanProperty('automatedTest')) { - wpi.sim.addGui().defaultEnabled = true - wpi.sim.addDriverstation() -} - -// Setting up my Jar File. In this case, adding all libraries into the main jar ("fat jar") +// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') // in order to make them all available at runtime. Also adding the manifest so WPILib // knows where to look for our Robot Class. jar { - from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } - manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) - duplicatesStrategy = DuplicatesStrategy.INCLUDE + from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } + from sourceSets.main.allSource + manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) + duplicatesStrategy = DuplicatesStrategy.INCLUDE } // Configure jar and deploy tasks @@ -114,26 +95,7 @@ deployArtifact.jarTask = jar wpi.java.configureExecutableTasks(jar) wpi.java.configureTestTasks(test) -// Spotless -spotless { - format "misc", { - target "*" - targetExclude "simgui*.json", "**/build/**", "**/build-*/**", "**/bin/**", "/.*" - toggleOffOn() - indentWithTabs() - endWithNewline() - trimTrailingWhitespace() - } - - java { - importOrder(); - googleJavaFormat(); - toggleOffOn(); - indentWithTabs(2); - endWithNewline(); - trimTrailingWhitespace(); - } +// Configure string concat to always inline compile +tasks.withType(JavaCompile) { + options.compilerArgs.add '-XDstringConcat=inline' } -tasks.named("build") { finalizedBy("spotlessApply") } -tasks.named("deploy") { finalizedBy("spotlessApply") } -spotlessCheck.mustRunAfter(compileJava); diff --git a/gradle.properties b/gradle.properties deleted file mode 100644 index fdc04e6e..00000000 --- a/gradle.properties +++ /dev/null @@ -1,6 +0,0 @@ -# Workaround for bug in google's remove unused imports on java 16/17 -org.gradle.jvmargs=--add-exports jdk.compiler/com.sun.tools.javac.api=ALL-UNNAMED \ ---add-exports jdk.compiler/com.sun.tools.javac.file=ALL-UNNAMED \ ---add-exports jdk.compiler/com.sun.tools.javac.parser=ALL-UNNAMED \ ---add-exports jdk.compiler/com.sun.tools.javac.tree=ALL-UNNAMED \ ---add-exports jdk.compiler/com.sun.tools.javac.util=ALL-UNNAMED diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar index 7454180f2ae8848c63b8b4dea2cb829da983f2fa..d64cd4917707c1f8861d8cb53dd15194d4248596 100644 GIT binary patch literal 43462 zcma&NWl&^owk(X(xVyW%ySuwf;qI=D6|RlDJ2cR^yEKh!@I- zp9QeisK*rlxC>+~7Dk4IxIRsKBHqdR9b3+fyL=ynHmIDe&|>O*VlvO+%z5;9Z$|DJ zb4dO}-R=MKr^6EKJiOrJdLnCJn>np?~vU-1sSFgPu;pthGwf}bG z(1db%xwr#x)r+`4AGu$j7~u2MpVs3VpLp|mx&;>`0p0vH6kF+D2CY0fVdQOZ@h;A` z{infNyvmFUiu*XG}RNMNwXrbec_*a3N=2zJ|Wh5z* z5rAX$JJR{#zP>KY**>xHTuw?|-Rg|o24V)74HcfVT;WtQHXlE+_4iPE8QE#DUm%x0 zEKr75ur~W%w#-My3Tj`hH6EuEW+8K-^5P62$7Sc5OK+22qj&Pd1;)1#4tKihi=~8C zHiQSst0cpri6%OeaR`PY>HH_;CPaRNty%WTm4{wDK8V6gCZlG@U3$~JQZ;HPvDJcT1V{ z?>H@13MJcCNe#5z+MecYNi@VT5|&UiN1D4ATT+%M+h4c$t;C#UAs3O_q=GxK0}8%8 z8J(_M9bayxN}69ex4dzM_P3oh@ZGREjVvn%%r7=xjkqxJP4kj}5tlf;QosR=%4L5y zWhgejO=vao5oX%mOHbhJ8V+SG&K5dABn6!WiKl{|oPkq(9z8l&Mm%(=qGcFzI=eLu zWc_oCLyf;hVlB@dnwY98?75B20=n$>u3b|NB28H0u-6Rpl((%KWEBOfElVWJx+5yg z#SGqwza7f}$z;n~g%4HDU{;V{gXIhft*q2=4zSezGK~nBgu9-Q*rZ#2f=Q}i2|qOp z!!y4p)4o=LVUNhlkp#JL{tfkhXNbB=Ox>M=n6soptJw-IDI|_$is2w}(XY>a=H52d z3zE$tjPUhWWS+5h=KVH&uqQS=$v3nRs&p$%11b%5qtF}S2#Pc`IiyBIF4%A!;AVoI zXU8-Rpv!DQNcF~(qQnyyMy=-AN~U>#&X1j5BLDP{?K!%h!;hfJI>$mdLSvktEr*89 zdJHvby^$xEX0^l9g$xW-d?J;L0#(`UT~zpL&*cEh$L|HPAu=P8`OQZV!-}l`noSp_ zQ-1$q$R-gDL)?6YaM!=8H=QGW$NT2SeZlb8PKJdc=F-cT@j7Xags+Pr*jPtlHFnf- zh?q<6;)27IdPc^Wdy-mX%2s84C1xZq9Xms+==F4);O`VUASmu3(RlgE#0+#giLh-& zcxm3_e}n4{%|X zJp{G_j+%`j_q5}k{eW&TlP}J2wtZ2^<^E(O)4OQX8FDp6RJq!F{(6eHWSD3=f~(h} zJXCf7=r<16X{pHkm%yzYI_=VDP&9bmI1*)YXZeB}F? z(%QsB5fo*FUZxK$oX~X^69;x~j7ms8xlzpt-T15e9}$4T-pC z6PFg@;B-j|Ywajpe4~bk#S6(fO^|mm1hKOPfA%8-_iGCfICE|=P_~e;Wz6my&)h_~ zkv&_xSAw7AZ%ThYF(4jADW4vg=oEdJGVOs>FqamoL3Np8>?!W#!R-0%2Bg4h?kz5I zKV-rKN2n(vUL%D<4oj@|`eJ>0i#TmYBtYmfla;c!ATW%;xGQ0*TW@PTlGG><@dxUI zg>+3SiGdZ%?5N=8uoLA|$4isK$aJ%i{hECP$bK{J#0W2gQ3YEa zZQ50Stn6hqdfxJ*9#NuSLwKFCUGk@c=(igyVL;;2^wi4o30YXSIb2g_ud$ zgpCr@H0qWtk2hK8Q|&wx)}4+hTYlf;$a4#oUM=V@Cw#!$(nOFFpZ;0lc!qd=c$S}Z zGGI-0jg~S~cgVT=4Vo)b)|4phjStD49*EqC)IPwyeKBLcN;Wu@Aeph;emROAwJ-0< z_#>wVm$)ygH|qyxZaet&(Vf%pVdnvKWJn9`%DAxj3ot;v>S$I}jJ$FLBF*~iZ!ZXE zkvui&p}fI0Y=IDX)mm0@tAd|fEHl~J&K}ZX(Mm3cm1UAuwJ42+AO5@HwYfDH7ipIc zmI;1J;J@+aCNG1M`Btf>YT>~c&3j~Qi@Py5JT6;zjx$cvOQW@3oQ>|}GH?TW-E z1R;q^QFjm5W~7f}c3Ww|awg1BAJ^slEV~Pk`Kd`PS$7;SqJZNj->it4DW2l15}xP6 zoCl$kyEF%yJni0(L!Z&14m!1urXh6Btj_5JYt1{#+H8w?5QI%% zo-$KYWNMJVH?Hh@1n7OSu~QhSswL8x0=$<8QG_zepi_`y_79=nK=_ZP_`Em2UI*tyQoB+r{1QYZCpb?2OrgUw#oRH$?^Tj!Req>XiE#~B|~ z+%HB;=ic+R@px4Ld8mwpY;W^A%8%l8$@B@1m5n`TlKI6bz2mp*^^^1mK$COW$HOfp zUGTz-cN9?BGEp}5A!mDFjaiWa2_J2Iq8qj0mXzk; z66JBKRP{p%wN7XobR0YjhAuW9T1Gw3FDvR5dWJ8ElNYF94eF3ebu+QwKjtvVu4L zI9ip#mQ@4uqVdkl-TUQMb^XBJVLW(-$s;Nq;@5gr4`UfLgF$adIhd?rHOa%D);whv z=;krPp~@I+-Z|r#s3yCH+c1US?dnm+C*)r{m+86sTJusLdNu^sqLrfWed^ndHXH`m zd3#cOe3>w-ga(Dus_^ppG9AC>Iq{y%%CK+Cro_sqLCs{VLuK=dev>OL1dis4(PQ5R zcz)>DjEkfV+MO;~>VUlYF00SgfUo~@(&9$Iy2|G0T9BSP?&T22>K46D zL*~j#yJ?)^*%J3!16f)@Y2Z^kS*BzwfAQ7K96rFRIh>#$*$_Io;z>ux@}G98!fWR@ zGTFxv4r~v)Gsd|pF91*-eaZ3Qw1MH$K^7JhWIdX%o$2kCbvGDXy)a?@8T&1dY4`;L z4Kn+f%SSFWE_rpEpL9bnlmYq`D!6F%di<&Hh=+!VI~j)2mfil03T#jJ_s?}VV0_hp z7T9bWxc>Jm2Z0WMU?`Z$xE74Gu~%s{mW!d4uvKCx@WD+gPUQ zV0vQS(Ig++z=EHN)BR44*EDSWIyT~R4$FcF*VEY*8@l=218Q05D2$|fXKFhRgBIEE zdDFB}1dKkoO^7}{5crKX!p?dZWNz$m>1icsXG2N+((x0OIST9Zo^DW_tytvlwXGpn zs8?pJXjEG;T@qrZi%#h93?FP$!&P4JA(&H61tqQi=opRzNpm zkrG}$^t9&XduK*Qa1?355wd8G2CI6QEh@Ua>AsD;7oRUNLPb76m4HG3K?)wF~IyS3`fXuNM>${?wmB zpVz;?6_(Fiadfd{vUCBM*_kt$+F3J+IojI;9L(gc9n3{sEZyzR9o!_mOwFC#tQ{Q~ zP3-`#uK#tP3Q7~Q;4H|wjZHO8h7e4IuBxl&vz2w~D8)w=Wtg31zpZhz%+kzSzL*dV zwp@{WU4i;hJ7c2f1O;7Mz6qRKeASoIv0_bV=i@NMG*l<#+;INk-^`5w@}Dj~;k=|}qM1vq_P z|GpBGe_IKq|LNy9SJhKOQ$c=5L{Dv|Q_lZl=-ky*BFBJLW9&y_C|!vyM~rQx=!vun z?rZJQB5t}Dctmui5i31C_;_}CEn}_W%>oSXtt>@kE1=JW*4*v4tPp;O6 zmAk{)m!)}34pTWg8{i>($%NQ(Tl;QC@J@FfBoc%Gr&m560^kgSfodAFrIjF}aIw)X zoXZ`@IsMkc8_=w%-7`D6Y4e*CG8k%Ud=GXhsTR50jUnm+R*0A(O3UKFg0`K;qp1bl z7``HN=?39ic_kR|^R^~w-*pa?Vj#7|e9F1iRx{GN2?wK!xR1GW!qa=~pjJb-#u1K8 zeR?Y2i-pt}yJq;SCiVHODIvQJX|ZJaT8nO+(?HXbLefulKKgM^B(UIO1r+S=7;kLJ zcH}1J=Px2jsh3Tec&v8Jcbng8;V-`#*UHt?hB(pmOipKwf3Lz8rG$heEB30Sg*2rx zV<|KN86$soN(I!BwO`1n^^uF2*x&vJ$2d$>+`(romzHP|)K_KkO6Hc>_dwMW-M(#S zK(~SiXT1@fvc#U+?|?PniDRm01)f^#55;nhM|wi?oG>yBsa?~?^xTU|fX-R(sTA+5 zaq}-8Tx7zrOy#3*JLIIVsBmHYLdD}!0NP!+ITW+Thn0)8SS!$@)HXwB3tY!fMxc#1 zMp3H?q3eD?u&Njx4;KQ5G>32+GRp1Ee5qMO0lZjaRRu&{W<&~DoJNGkcYF<5(Ab+J zgO>VhBl{okDPn78<%&e2mR{jwVCz5Og;*Z;;3%VvoGo_;HaGLWYF7q#jDX=Z#Ml`H z858YVV$%J|e<1n`%6Vsvq7GmnAV0wW4$5qQ3uR@1i>tW{xrl|ExywIc?fNgYlA?C5 zh$ezAFb5{rQu6i7BSS5*J-|9DQ{6^BVQ{b*lq`xS@RyrsJN?-t=MTMPY;WYeKBCNg z^2|pN!Q^WPJuuO4!|P@jzt&tY1Y8d%FNK5xK(!@`jO2aEA*4 zkO6b|UVBipci?){-Ke=+1;mGlND8)6+P;8sq}UXw2hn;fc7nM>g}GSMWu&v&fqh

iViYT=fZ(|3Ox^$aWPp4a8h24tD<|8-!aK0lHgL$N7Efw}J zVIB!7=T$U`ao1?upi5V4Et*-lTG0XvExbf!ya{cua==$WJyVG(CmA6Of*8E@DSE%L z`V^$qz&RU$7G5mg;8;=#`@rRG`-uS18$0WPN@!v2d{H2sOqP|!(cQ@ zUHo!d>>yFArLPf1q`uBvY32miqShLT1B@gDL4XoVTK&@owOoD)OIHXrYK-a1d$B{v zF^}8D3Y^g%^cnvScOSJR5QNH+BI%d|;J;wWM3~l>${fb8DNPg)wrf|GBP8p%LNGN# z3EaIiItgwtGgT&iYCFy9-LG}bMI|4LdmmJt@V@% zb6B)1kc=T)(|L@0;wr<>=?r04N;E&ef+7C^`wPWtyQe(*pD1pI_&XHy|0gIGHMekd zF_*M4yi6J&Z4LQj65)S zXwdM{SwUo%3SbPwFsHgqF@V|6afT|R6?&S;lw=8% z3}@9B=#JI3@B*#4s!O))~z zc>2_4Q_#&+5V`GFd?88^;c1i7;Vv_I*qt!_Yx*n=;rj!82rrR2rQ8u5(Ejlo{15P% zs~!{%XJ>FmJ})H^I9bn^Re&38H{xA!0l3^89k(oU;bZWXM@kn$#aoS&Y4l^-WEn-fH39Jb9lA%s*WsKJQl?n9B7_~P z-XM&WL7Z!PcoF6_D>V@$CvUIEy=+Z&0kt{szMk=f1|M+r*a43^$$B^MidrT0J;RI` z(?f!O<8UZkm$_Ny$Hth1J#^4ni+im8M9mr&k|3cIgwvjAgjH z8`N&h25xV#v*d$qBX5jkI|xOhQn!>IYZK7l5#^P4M&twe9&Ey@@GxYMxBZq2e7?`q z$~Szs0!g{2fGcp9PZEt|rdQ6bhAgpcLHPz?f-vB?$dc*!9OL?Q8mn7->bFD2Si60* z!O%y)fCdMSV|lkF9w%x~J*A&srMyYY3{=&$}H zGQ4VG_?$2X(0|vT0{=;W$~icCI{b6W{B!Q8xdGhF|D{25G_5_+%s(46lhvNLkik~R z>nr(&C#5wwOzJZQo9m|U<;&Wk!_#q|V>fsmj1g<6%hB{jGoNUPjgJslld>xmODzGjYc?7JSuA?A_QzjDw5AsRgi@Y|Z0{F{!1=!NES-#*f^s4l0Hu zz468))2IY5dmD9pa*(yT5{EyP^G>@ZWumealS-*WeRcZ}B%gxq{MiJ|RyX-^C1V=0 z@iKdrGi1jTe8Ya^x7yyH$kBNvM4R~`fbPq$BzHum-3Zo8C6=KW@||>zsA8-Y9uV5V z#oq-f5L5}V<&wF4@X@<3^C%ptp6+Ce)~hGl`kwj)bsAjmo_GU^r940Z-|`<)oGnh7 zFF0Tde3>ui?8Yj{sF-Z@)yQd~CGZ*w-6p2U<8}JO-sRsVI5dBji`01W8A&3$?}lxBaC&vn0E$c5tW* zX>5(zzZ=qn&!J~KdsPl;P@bmA-Pr8T*)eh_+Dv5=Ma|XSle6t(k8qcgNyar{*ReQ8 zTXwi=8vr>!3Ywr+BhggHDw8ke==NTQVMCK`$69fhzEFB*4+H9LIvdt-#IbhZvpS}} zO3lz;P?zr0*0$%-Rq_y^k(?I{Mk}h@w}cZpMUp|ucs55bcloL2)($u%mXQw({Wzc~ z;6nu5MkjP)0C(@%6Q_I_vsWrfhl7Zpoxw#WoE~r&GOSCz;_ro6i(^hM>I$8y>`!wW z*U^@?B!MMmb89I}2(hcE4zN2G^kwyWCZp5JG>$Ez7zP~D=J^LMjSM)27_0B_X^C(M z`fFT+%DcKlu?^)FCK>QzSnV%IsXVcUFhFdBP!6~se&xxrIxsvySAWu++IrH;FbcY$ z2DWTvSBRfLwdhr0nMx+URA$j3i7_*6BWv#DXfym?ZRDcX9C?cY9sD3q)uBDR3uWg= z(lUIzB)G$Hr!){>E{s4Dew+tb9kvToZp-1&c?y2wn@Z~(VBhqz`cB;{E4(P3N2*nJ z_>~g@;UF2iG{Kt(<1PyePTKahF8<)pozZ*xH~U-kfoAayCwJViIrnqwqO}7{0pHw$ zs2Kx?s#vQr7XZ264>5RNKSL8|Ty^=PsIx^}QqOOcfpGUU4tRkUc|kc7-!Ae6!+B{o~7nFpm3|G5^=0#Bnm6`V}oSQlrX(u%OWnC zoLPy&Q;1Jui&7ST0~#+}I^&?vcE*t47~Xq#YwvA^6^} z`WkC)$AkNub|t@S!$8CBlwbV~?yp&@9h{D|3z-vJXgzRC5^nYm+PyPcgRzAnEi6Q^gslXYRv4nycsy-SJu?lMps-? zV`U*#WnFsdPLL)Q$AmD|0`UaC4ND07+&UmOu!eHruzV|OUox<+Jl|Mr@6~C`T@P%s zW7sgXLF2SSe9Fl^O(I*{9wsFSYb2l%-;&Pi^dpv!{)C3d0AlNY6!4fgmSgj_wQ*7Am7&$z;Jg&wgR-Ih;lUvWS|KTSg!&s_E9_bXBkZvGiC6bFKDWZxsD$*NZ#_8bl zG1P-#@?OQzED7@jlMJTH@V!6k;W>auvft)}g zhoV{7$q=*;=l{O>Q4a@ ziMjf_u*o^PsO)#BjC%0^h>Xp@;5$p{JSYDt)zbb}s{Kbt!T*I@Pk@X0zds6wsefuU zW$XY%yyRGC94=6mf?x+bbA5CDQ2AgW1T-jVAJbm7K(gp+;v6E0WI#kuACgV$r}6L? zd|Tj?^%^*N&b>Dd{Wr$FS2qI#Ucs1yd4N+RBUQiSZGujH`#I)mG&VKoDh=KKFl4=G z&MagXl6*<)$6P}*Tiebpz5L=oMaPrN+caUXRJ`D?=K9!e0f{@D&cZLKN?iNP@X0aF zE(^pl+;*T5qt?1jRC=5PMgV!XNITRLS_=9{CJExaQj;lt!&pdzpK?8p>%Mb+D z?yO*uSung=-`QQ@yX@Hyd4@CI^r{2oiu`%^bNkz+Nkk!IunjwNC|WcqvX~k=><-I3 zDQdbdb|!v+Iz01$w@aMl!R)koD77Xp;eZwzSl-AT zr@Vu{=xvgfq9akRrrM)}=!=xcs+U1JO}{t(avgz`6RqiiX<|hGG1pmop8k6Q+G_mv zJv|RfDheUp2L3=^C=4aCBMBn0aRCU(DQwX-W(RkRwmLeuJYF<0urcaf(=7)JPg<3P zQs!~G)9CT18o!J4{zX{_e}4eS)U-E)0FAt}wEI(c0%HkxgggW;(1E=>J17_hsH^sP z%lT0LGgbUXHx-K*CI-MCrP66UP0PvGqM$MkeLyqHdbgP|_Cm!7te~b8p+e6sQ_3k| zVcwTh6d83ltdnR>D^)BYQpDKlLk3g0Hdcgz2}%qUs9~~Rie)A-BV1mS&naYai#xcZ z(d{8=-LVpTp}2*y)|gR~;qc7fp26}lPcLZ#=JpYcn3AT9(UIdOyg+d(P5T7D&*P}# zQCYplZO5|7+r19%9e`v^vfSS1sbX1c%=w1;oyruXB%Kl$ACgKQ6=qNWLsc=28xJjg zwvsI5-%SGU|3p>&zXVl^vVtQT3o-#$UT9LI@Npz~6=4!>mc431VRNN8od&Ul^+G_kHC`G=6WVWM z%9eWNyy(FTO|A+@x}Ou3CH)oi;t#7rAxdIXfNFwOj_@Y&TGz6P_sqiB`Q6Lxy|Q{`|fgmRG(k+!#b*M+Z9zFce)f-7;?Km5O=LHV9f9_87; zF7%R2B+$?@sH&&-$@tzaPYkw0;=i|;vWdI|Wl3q_Zu>l;XdIw2FjV=;Mq5t1Q0|f< zs08j54Bp`3RzqE=2enlkZxmX6OF+@|2<)A^RNQpBd6o@OXl+i)zO%D4iGiQNuXd+zIR{_lb96{lc~bxsBveIw6umhShTX+3@ZJ=YHh@ zWY3(d0azg;7oHn>H<>?4@*RQbi>SmM=JrHvIG(~BrvI)#W(EAeO6fS+}mxxcc+X~W6&YVl86W9WFSS}Vz-f9vS?XUDBk)3TcF z8V?$4Q)`uKFq>xT=)Y9mMFVTUk*NIA!0$?RP6Ig0TBmUFrq*Q-Agq~DzxjStQyJ({ zBeZ;o5qUUKg=4Hypm|}>>L=XKsZ!F$yNTDO)jt4H0gdQ5$f|d&bnVCMMXhNh)~mN z@_UV6D7MVlsWz+zM+inZZp&P4fj=tm6fX)SG5H>OsQf_I8c~uGCig$GzuwViK54bcgL;VN|FnyQl>Ed7(@>=8$a_UKIz|V6CeVSd2(P z0Uu>A8A+muM%HLFJQ9UZ5c)BSAv_zH#1f02x?h9C}@pN@6{>UiAp>({Fn(T9Q8B z^`zB;kJ5b`>%dLm+Ol}ty!3;8f1XDSVX0AUe5P#@I+FQ-`$(a;zNgz)4x5hz$Hfbg z!Q(z26wHLXko(1`;(BAOg_wShpX0ixfWq3ponndY+u%1gyX)_h=v1zR#V}#q{au6; z!3K=7fQwnRfg6FXtNQmP>`<;!N137paFS%y?;lb1@BEdbvQHYC{976l`cLqn;b8lp zIDY>~m{gDj(wfnK!lpW6pli)HyLEiUrNc%eXTil|F2s(AY+LW5hkKb>TQ3|Q4S9rr zpDs4uK_co6XPsn_z$LeS{K4jFF`2>U`tbgKdyDne`xmR<@6AA+_hPNKCOR-Zqv;xk zu5!HsBUb^!4uJ7v0RuH-7?l?}b=w5lzzXJ~gZcxRKOovSk@|#V+MuX%Y+=;14i*%{)_gSW9(#4%)AV#3__kac1|qUy!uyP{>?U#5wYNq}y$S9pCc zFc~4mgSC*G~j0u#qqp9 z${>3HV~@->GqEhr_Xwoxq?Hjn#=s2;i~g^&Hn|aDKpA>Oc%HlW(KA1?BXqpxB;Ydx)w;2z^MpjJ(Qi(X!$5RC z*P{~%JGDQqojV>2JbEeCE*OEu!$XJ>bWA9Oa_Hd;y)F%MhBRi*LPcdqR8X`NQ&1L# z5#9L*@qxrx8n}LfeB^J{%-?SU{FCwiWyHp682F+|pa+CQa3ZLzBqN1{)h4d6+vBbV zC#NEbQLC;}me3eeYnOG*nXOJZEU$xLZ1<1Y=7r0(-U0P6-AqwMAM`a(Ed#7vJkn6plb4eI4?2y3yOTGmmDQ!z9`wzbf z_OY#0@5=bnep;MV0X_;;SJJWEf^E6Bd^tVJ9znWx&Ks8t*B>AM@?;D4oWUGc z!H*`6d7Cxo6VuyS4Eye&L1ZRhrRmN6Lr`{NL(wDbif|y&z)JN>Fl5#Wi&mMIr5i;x zBx}3YfF>>8EC(fYnmpu~)CYHuHCyr5*`ECap%t@y=jD>!_%3iiE|LN$mK9>- zHdtpy8fGZtkZF?%TW~29JIAfi2jZT8>OA7=h;8T{{k?c2`nCEx9$r zS+*&vt~2o^^J+}RDG@+9&M^K*z4p{5#IEVbz`1%`m5c2};aGt=V?~vIM}ZdPECDI)47|CWBCfDWUbxBCnmYivQ*0Nu_xb*C>~C9(VjHM zxe<*D<#dQ8TlpMX2c@M<9$w!RP$hpG4cs%AI){jp*Sj|*`m)5(Bw*A0$*i-(CA5#%>a)$+jI2C9r6|(>J8InryENI z$NohnxDUB;wAYDwrb*!N3noBTKPpPN}~09SEL18tkG zxgz(RYU_;DPT{l?Q$+eaZaxnsWCA^ds^0PVRkIM%bOd|G2IEBBiz{&^JtNsODs;5z zICt_Zj8wo^KT$7Bg4H+y!Df#3mbl%%?|EXe!&(Vmac1DJ*y~3+kRKAD=Ovde4^^%~ zw<9av18HLyrf*_>Slp;^i`Uy~`mvBjZ|?Ad63yQa#YK`4+c6;pW4?XIY9G1(Xh9WO8{F-Aju+nS9Vmv=$Ac0ienZ+p9*O%NG zMZKy5?%Z6TAJTE?o5vEr0r>f>hb#2w2U3DL64*au_@P!J!TL`oH2r*{>ffu6|A7tv zL4juf$DZ1MW5ZPsG!5)`k8d8c$J$o;%EIL0va9&GzWvkS%ZsGb#S(?{!UFOZ9<$a| zY|a+5kmD5N&{vRqkgY>aHsBT&`rg|&kezoD)gP0fsNYHsO#TRc_$n6Lf1Z{?+DLziXlHrq4sf(!>O{?Tj;Eh@%)+nRE_2VxbN&&%%caU#JDU%vL3}Cb zsb4AazPI{>8H&d=jUaZDS$-0^AxE@utGs;-Ez_F(qC9T=UZX=>ok2k2 ziTn{K?y~a5reD2A)P${NoI^>JXn>`IeArow(41c-Wm~)wiryEP(OS{YXWi7;%dG9v zI?mwu1MxD{yp_rrk!j^cKM)dc4@p4Ezyo%lRN|XyD}}>v=Xoib0gOcdXrQ^*61HNj z=NP|pd>@yfvr-=m{8$3A8TQGMTE7g=z!%yt`8`Bk-0MMwW~h^++;qyUP!J~ykh1GO z(FZ59xuFR$(WE;F@UUyE@Sp>`aVNjyj=Ty>_Vo}xf`e7`F;j-IgL5`1~-#70$9_=uBMq!2&1l zomRgpD58@)YYfvLtPW}{C5B35R;ZVvB<<#)x%srmc_S=A7F@DW8>QOEGwD6suhwCg z>Pa+YyULhmw%BA*4yjDp|2{!T98~<6Yfd(wo1mQ!KWwq0eg+6)o1>W~f~kL<-S+P@$wx*zeI|1t7z#Sxr5 zt6w+;YblPQNplq4Z#T$GLX#j6yldXAqj>4gAnnWtBICUnA&-dtnlh=t0Ho_vEKwV` z)DlJi#!@nkYV#$!)@>udAU*hF?V`2$Hf=V&6PP_|r#Iv*J$9)pF@X3`k;5})9^o4y z&)~?EjX5yX12O(BsFy-l6}nYeuKkiq`u9145&3Ssg^y{5G3Pse z9w(YVa0)N-fLaBq1`P!_#>SS(8fh_5!f{UrgZ~uEdeMJIz7DzI5!NHHqQtm~#CPij z?=N|J>nPR6_sL7!f4hD_|KH`vf8(Wpnj-(gPWH+ZvID}%?~68SwhPTC3u1_cB`otq z)U?6qo!ZLi5b>*KnYHWW=3F!p%h1;h{L&(Q&{qY6)_qxNfbP6E3yYpW!EO+IW3?@J z);4>g4gnl^8klu7uA>eGF6rIGSynacogr)KUwE_R4E5Xzi*Qir@b-jy55-JPC8c~( zo!W8y9OGZ&`xmc8;=4-U9=h{vCqfCNzYirONmGbRQlR`WWlgnY+1wCXbMz&NT~9*| z6@FrzP!LX&{no2!Ln_3|I==_4`@}V?4a;YZKTdw;vT<+K+z=uWbW(&bXEaWJ^W8Td z-3&1bY^Z*oM<=M}LVt>_j+p=2Iu7pZmbXrhQ_k)ysE9yXKygFNw$5hwDn(M>H+e1&9BM5!|81vd%r%vEm zqxY3?F@fb6O#5UunwgAHR9jp_W2zZ}NGp2%mTW@(hz7$^+a`A?mb8|_G*GNMJ) zjqegXQio=i@AINre&%ofexAr95aop5C+0MZ0m-l=MeO8m3epm7U%vZB8+I+C*iNFM z#T3l`gknX;D$-`2XT^Cg*vrv=RH+P;_dfF++cP?B_msQI4j+lt&rX2)3GaJx%W*Nn zkML%D{z5tpHH=dksQ*gzc|}gzW;lwAbxoR07VNgS*-c3d&8J|;@3t^ zVUz*J*&r7DFRuFVDCJDK8V9NN5hvpgGjwx+5n)qa;YCKe8TKtdnh{I7NU9BCN!0dq zczrBk8pE{{@vJa9ywR@mq*J=v+PG;?fwqlJVhijG!3VmIKs>9T6r7MJpC)m!Tc#>g zMtVsU>wbwFJEfwZ{vB|ZlttNe83)$iz`~#8UJ^r)lJ@HA&G#}W&ZH*;k{=TavpjWE z7hdyLZPf*X%Gm}i`Y{OGeeu^~nB8=`{r#TUrM-`;1cBvEd#d!kPqIgYySYhN-*1;L z^byj%Yi}Gx)Wnkosi337BKs}+5H5dth1JA{Ir-JKN$7zC)*}hqeoD(WfaUDPT>0`- z(6sa0AoIqASwF`>hP}^|)a_j2s^PQn*qVC{Q}htR z5-)duBFXT_V56-+UohKXlq~^6uf!6sA#ttk1o~*QEy_Y-S$gAvq47J9Vtk$5oA$Ct zYhYJ@8{hsC^98${!#Ho?4y5MCa7iGnfz}b9jE~h%EAAv~Qxu)_rAV;^cygV~5r_~?l=B`zObj7S=H=~$W zPtI_m%g$`kL_fVUk9J@>EiBH zOO&jtn~&`hIFMS5S`g8w94R4H40mdNUH4W@@XQk1sr17b{@y|JB*G9z1|CrQjd+GX z6+KyURG3;!*BQrentw{B2R&@2&`2}n(z-2&X7#r!{yg@Soy}cRD~j zj9@UBW+N|4HW4AWapy4wfUI- zZ`gSL6DUlgj*f1hSOGXG0IVH8HxK?o2|3HZ;KW{K+yPAlxtb)NV_2AwJm|E)FRs&& z=c^e7bvUsztY|+f^k7NXs$o1EUq>cR7C0$UKi6IooHWlK_#?IWDkvywnzg&ThWo^? z2O_N{5X39#?eV9l)xI(>@!vSB{DLt*oY!K1R8}_?%+0^C{d9a%N4 zoxHVT1&Lm|uDX%$QrBun5e-F`HJ^T$ zmzv)p@4ZHd_w9!%Hf9UYNvGCw2TTTbrj9pl+T9%-_-}L(tES>Or-}Z4F*{##n3~L~TuxjirGuIY#H7{%$E${?p{Q01 zi6T`n;rbK1yIB9jmQNycD~yZq&mbIsFWHo|ZAChSFPQa<(%d8mGw*V3fh|yFoxOOiWJd(qvVb!Z$b88cg->N=qO*4k~6;R==|9ihg&riu#P~s4Oap9O7f%crSr^rljeIfXDEg>wi)&v*a%7zpz<9w z*r!3q9J|390x`Zk;g$&OeN&ctp)VKRpDSV@kU2Q>jtok($Y-*x8_$2piTxun81@vt z!Vj?COa0fg2RPXMSIo26T=~0d`{oGP*eV+$!0I<(4azk&Vj3SiG=Q!6mX0p$z7I}; z9BJUFgT-K9MQQ-0@Z=^7R<{bn2Fm48endsSs`V7_@%8?Bxkqv>BDoVcj?K#dV#uUP zL1ND~?D-|VGKe3Rw_7-Idpht>H6XRLh*U7epS6byiGvJpr%d}XwfusjH9g;Z98H`x zyde%%5mhGOiL4wljCaWCk-&uE4_OOccb9c!ZaWt4B(wYl!?vyzl%7n~QepN&eFUrw zFIOl9c({``6~QD+43*_tzP{f2x41h(?b43^y6=iwyB)2os5hBE!@YUS5?N_tXd=h( z)WE286Fbd>R4M^P{!G)f;h<3Q>Fipuy+d2q-)!RyTgt;wr$(?9ox3;q+{E*ZQHhOn;lM`cjnu9 zXa48ks-v(~b*;MAI<>YZH(^NV8vjb34beE<_cwKlJoR;k6lJNSP6v}uiyRD?|0w+X@o1ONrH8a$fCxXpf? z?$DL0)7|X}Oc%h^zrMKWc-NS9I0Utu@>*j}b@tJ=ixQSJ={4@854wzW@E>VSL+Y{i z#0b=WpbCZS>kUCO_iQz)LoE>P5LIG-hv9E+oG}DtlIDF>$tJ1aw9^LuhLEHt?BCj& z(O4I8v1s#HUi5A>nIS-JK{v!7dJx)^Yg%XjNmlkWAq2*cv#tHgz`Y(bETc6CuO1VkN^L-L3j_x<4NqYb5rzrLC-7uOv z!5e`GZt%B782C5-fGnn*GhDF$%(qP<74Z}3xx+{$4cYKy2ikxI7B2N+2r07DN;|-T->nU&!=Cm#rZt%O_5c&1Z%nlWq3TKAW0w zQqemZw_ue--2uKQsx+niCUou?HjD`xhEjjQd3%rrBi82crq*~#uA4+>vR<_S{~5ce z-2EIl?~s z1=GVL{NxP1N3%=AOaC}j_Fv=ur&THz zyO!d9kHq|c73kpq`$+t+8Bw7MgeR5~`d7ChYyGCBWSteTB>8WAU(NPYt2Dk`@#+}= zI4SvLlyk#pBgVigEe`?NG*vl7V6m+<}%FwPV=~PvvA)=#ths==DRTDEYh4V5}Cf$z@#;< zyWfLY_5sP$gc3LLl2x+Ii)#b2nhNXJ{R~vk`s5U7Nyu^3yFg&D%Txwj6QezMX`V(x z=C`{76*mNb!qHHs)#GgGZ_7|vkt9izl_&PBrsu@}L`X{95-2jf99K)0=*N)VxBX2q z((vkpP2RneSIiIUEnGb?VqbMb=Zia+rF~+iqslydE34cSLJ&BJW^3knX@M;t*b=EA zNvGzv41Ld_T+WT#XjDB840vovUU^FtN_)G}7v)1lPetgpEK9YS^OWFkPoE{ovj^=@ zO9N$S=G$1ecndT_=5ehth2Lmd1II-PuT~C9`XVePw$y8J#dpZ?Tss<6wtVglm(Ok7 z3?^oi@pPio6l&!z8JY(pJvG=*pI?GIOu}e^EB6QYk$#FJQ%^AIK$I4epJ+9t?KjqA+bkj&PQ*|vLttme+`9G=L% ziadyMw_7-M)hS(3E$QGNCu|o23|%O+VN7;Qggp?PB3K-iSeBa2b}V4_wY`G1Jsfz4 z9|SdB^;|I8E8gWqHKx!vj_@SMY^hLEIbSMCuE?WKq=c2mJK z8LoG-pnY!uhqFv&L?yEuxo{dpMTsmCn)95xanqBrNPTgXP((H$9N${Ow~Is-FBg%h z53;|Y5$MUN)9W2HBe2TD`ct^LHI<(xWrw}$qSoei?}s)&w$;&!14w6B6>Yr6Y8b)S z0r71`WmAvJJ`1h&poLftLUS6Ir zC$bG9!Im_4Zjse)#K=oJM9mHW1{%l8sz$1o?ltdKlLTxWWPB>Vk22czVt|1%^wnN@*!l)}?EgtvhC>vlHm^t+ogpgHI1_$1ox9e;>0!+b(tBrmXRB`PY1vp-R**8N7 zGP|QqI$m(Rdu#=(?!(N}G9QhQ%o!aXE=aN{&wtGP8|_qh+7a_j_sU5|J^)vxq;# zjvzLn%_QPHZZIWu1&mRAj;Sa_97p_lLq_{~j!M9N^1yp3U_SxRqK&JnR%6VI#^E12 z>CdOVI^_9aPK2eZ4h&^{pQs}xsijXgFYRIxJ~N7&BB9jUR1fm!(xl)mvy|3e6-B3j zJn#ajL;bFTYJ2+Q)tDjx=3IklO@Q+FFM}6UJr6km7hj7th9n_&JR7fnqC!hTZoM~T zBeaVFp%)0cbPhejX<8pf5HyRUj2>aXnXBqDJe73~J%P(2C?-RT{c3NjE`)om! zl$uewSgWkE66$Kb34+QZZvRn`fob~Cl9=cRk@Es}KQm=?E~CE%spXaMO6YmrMl%9Q zlA3Q$3|L1QJ4?->UjT&CBd!~ru{Ih^in&JXO=|<6J!&qp zRe*OZ*cj5bHYlz!!~iEKcuE|;U4vN1rk$xq6>bUWD*u(V@8sG^7>kVuo(QL@Ki;yL zWC!FT(q{E8#on>%1iAS0HMZDJg{Z{^!De(vSIq&;1$+b)oRMwA3nc3mdTSG#3uYO_ z>+x;7p4I;uHz?ZB>dA-BKl+t-3IB!jBRgdvAbW!aJ(Q{aT>+iz?91`C-xbe)IBoND z9_Xth{6?(y3rddwY$GD65IT#f3<(0o#`di{sh2gm{dw*#-Vnc3r=4==&PU^hCv$qd zjw;>i&?L*Wq#TxG$mFIUf>eK+170KG;~+o&1;Tom9}}mKo23KwdEM6UonXgc z!6N(@k8q@HPw{O8O!lAyi{rZv|DpgfU{py+j(X_cwpKqcalcqKIr0kM^%Br3SdeD> zHSKV94Yxw;pjzDHo!Q?8^0bb%L|wC;4U^9I#pd5O&eexX+Im{ z?jKnCcsE|H?{uGMqVie_C~w7GX)kYGWAg%-?8|N_1#W-|4F)3YTDC+QSq1s!DnOML3@d`mG%o2YbYd#jww|jD$gotpa)kntakp#K;+yo-_ZF9qrNZw<%#C zuPE@#3RocLgPyiBZ+R_-FJ_$xP!RzWm|aN)S+{$LY9vvN+IW~Kf3TsEIvP+B9Mtm! zpfNNxObWQpLoaO&cJh5>%slZnHl_Q~(-Tfh!DMz(dTWld@LG1VRF`9`DYKhyNv z2pU|UZ$#_yUx_B_|MxUq^glT}O5Xt(Vm4Mr02><%C)@v;vPb@pT$*yzJ4aPc_FZ3z z3}PLoMBIM>q_9U2rl^sGhk1VUJ89=*?7|v`{!Z{6bqFMq(mYiA?%KbsI~JwuqVA9$H5vDE+VocjX+G^%bieqx->s;XWlKcuv(s%y%D5Xbc9+ zc(_2nYS1&^yL*ey664&4`IoOeDIig}y-E~_GS?m;D!xv5-xwz+G`5l6V+}CpeJDi^ z%4ed$qowm88=iYG+(`ld5Uh&>Dgs4uPHSJ^TngXP_V6fPyl~>2bhi20QB%lSd#yYn zO05?KT1z@?^-bqO8Cg`;ft>ilejsw@2%RR7;`$Vs;FmO(Yr3Fp`pHGr@P2hC%QcA|X&N2Dn zYf`MqXdHi%cGR@%y7Rg7?d3?an){s$zA{!H;Ie5exE#c~@NhQUFG8V=SQh%UxUeiV zd7#UcYqD=lk-}sEwlpu&H^T_V0{#G?lZMxL7ih_&{(g)MWBnCZxtXg znr#}>U^6!jA%e}@Gj49LWG@*&t0V>Cxc3?oO7LSG%~)Y5}f7vqUUnQ;STjdDU}P9IF9d9<$;=QaXc zL1^X7>fa^jHBu_}9}J~#-oz3Oq^JmGR#?GO7b9a(=R@fw@}Q{{@`Wy1vIQ#Bw?>@X z-_RGG@wt|%u`XUc%W{J z>iSeiz8C3H7@St3mOr_mU+&bL#Uif;+Xw-aZdNYUpdf>Rvu0i0t6k*}vwU`XNO2he z%miH|1tQ8~ZK!zmL&wa3E;l?!!XzgV#%PMVU!0xrDsNNZUWKlbiOjzH-1Uoxm8E#r`#2Sz;-o&qcqB zC-O_R{QGuynW14@)7&@yw1U}uP(1cov)twxeLus0s|7ayrtT8c#`&2~Fiu2=R;1_4bCaD=*E@cYI>7YSnt)nQc zohw5CsK%m?8Ack)qNx`W0_v$5S}nO|(V|RZKBD+btO?JXe|~^Qqur%@eO~<8-L^9d z=GA3-V14ng9L29~XJ>a5k~xT2152zLhM*@zlp2P5Eu}bywkcqR;ISbas&#T#;HZSf z2m69qTV(V@EkY(1Dk3`}j)JMo%ZVJ*5eB zYOjIisi+igK0#yW*gBGj?@I{~mUOvRFQR^pJbEbzFxTubnrw(Muk%}jI+vXmJ;{Q6 zrSobKD>T%}jV4Ub?L1+MGOD~0Ir%-`iTnWZN^~YPrcP5y3VMAzQ+&en^VzKEb$K!Q z<7Dbg&DNXuow*eD5yMr+#08nF!;%4vGrJI++5HdCFcGLfMW!KS*Oi@=7hFwDG!h2< zPunUEAF+HncQkbfFj&pbzp|MU*~60Z(|Ik%Tn{BXMN!hZOosNIseT?R;A`W?=d?5X zK(FB=9mZusYahp|K-wyb={rOpdn=@;4YI2W0EcbMKyo~-#^?h`BA9~o285%oY zfifCh5Lk$SY@|2A@a!T2V+{^!psQkx4?x0HSV`(w9{l75QxMk!)U52Lbhn{8ol?S) zCKo*7R(z!uk<6*qO=wh!Pul{(qq6g6xW;X68GI_CXp`XwO zxuSgPRAtM8K7}5E#-GM!*ydOOG_{A{)hkCII<|2=ma*71ci_-}VPARm3crFQjLYV! z9zbz82$|l01mv`$WahE2$=fAGWkd^X2kY(J7iz}WGS z@%MyBEO=A?HB9=^?nX`@nh;7;laAjs+fbo!|K^mE!tOB>$2a_O0y-*uaIn8k^6Y zSbuv;5~##*4Y~+y7Z5O*3w4qgI5V^17u*ZeupVGH^nM&$qmAk|anf*>r zWc5CV;-JY-Z@Uq1Irpb^O`L_7AGiqd*YpGUShb==os$uN3yYvb`wm6d=?T*it&pDk zo`vhw)RZX|91^^Wa_ti2zBFyWy4cJu#g)_S6~jT}CC{DJ_kKpT`$oAL%b^!2M;JgT zM3ZNbUB?}kP(*YYvXDIH8^7LUxz5oE%kMhF!rnPqv!GiY0o}NR$OD=ITDo9r%4E>E0Y^R(rS^~XjWyVI6 zMOR5rPXhTp*G*M&X#NTL`Hu*R+u*QNoiOKg4CtNPrjgH>c?Hi4MUG#I917fx**+pJfOo!zFM&*da&G_x)L(`k&TPI*t3e^{crd zX<4I$5nBQ8Ax_lmNRa~E*zS-R0sxkz`|>7q_?*e%7bxqNm3_eRG#1ae3gtV9!fQpY z+!^a38o4ZGy9!J5sylDxZTx$JmG!wg7;>&5H1)>f4dXj;B+@6tMlL=)cLl={jLMxY zbbf1ax3S4>bwB9-$;SN2?+GULu;UA-35;VY*^9Blx)Jwyb$=U!D>HhB&=jSsd^6yw zL)?a|>GxU!W}ocTC(?-%z3!IUhw^uzc`Vz_g>-tv)(XA#JK^)ZnC|l1`@CdX1@|!| z_9gQ)7uOf?cR@KDp97*>6X|;t@Y`k_N@)aH7gY27)COv^P3ya9I{4z~vUjLR9~z1Z z5=G{mVtKH*&$*t0@}-i_v|3B$AHHYale7>E+jP`ClqG%L{u;*ff_h@)al?RuL7tOO z->;I}>%WI{;vbLP3VIQ^iA$4wl6@0sDj|~112Y4OFjMs`13!$JGkp%b&E8QzJw_L5 zOnw9joc0^;O%OpF$Qp)W1HI!$4BaXX84`%@#^dk^hFp^pQ@rx4g(8Xjy#!X%+X5Jd@fs3amGT`}mhq#L97R>OwT5-m|h#yT_-v@(k$q7P*9X~T*3)LTdzP!*B} z+SldbVWrrwQo9wX*%FyK+sRXTa@O?WM^FGWOE?S`R(0P{<6p#f?0NJvnBia?k^fX2 zNQs7K-?EijgHJY}&zsr;qJ<*PCZUd*x|dD=IQPUK_nn)@X4KWtqoJNHkT?ZWL_hF? zS8lp2(q>;RXR|F;1O}EE#}gCrY~#n^O`_I&?&z5~7N;zL0)3Tup`%)oHMK-^r$NT% zbFg|o?b9w(q@)6w5V%si<$!U<#}s#x@0aX-hP>zwS#9*75VXA4K*%gUc>+yzupTDBOKH8WR4V0pM(HrfbQ&eJ79>HdCvE=F z|J>s;;iDLB^3(9}?biKbxf1$lI!*Z%*0&8UUq}wMyPs_hclyQQi4;NUY+x2qy|0J; zhn8;5)4ED1oHwg+VZF|80<4MrL97tGGXc5Sw$wAI#|2*cvQ=jB5+{AjMiDHmhUC*a zlmiZ`LAuAn_}hftXh;`Kq0zblDk8?O-`tnilIh|;3lZp@F_osJUV9`*R29M?7H{Fy z`nfVEIDIWXmU&YW;NjU8)EJpXhxe5t+scf|VXM!^bBlwNh)~7|3?fWwo_~ZFk(22% zTMesYw+LNx3J-_|DM~`v93yXe=jPD{q;li;5PD?Dyk+b? zo21|XpT@)$BM$%F=P9J19Vi&1#{jM3!^Y&fr&_`toi`XB1!n>sbL%U9I5<7!@?t)~ z;&H%z>bAaQ4f$wIzkjH70;<8tpUoxzKrPhn#IQfS%9l5=Iu))^XC<58D!-O z{B+o5R^Z21H0T9JQ5gNJnqh#qH^na|z92=hONIM~@_iuOi|F>jBh-?aA20}Qx~EpDGElELNn~|7WRXRFnw+Wdo`|# zBpU=Cz3z%cUJ0mx_1($X<40XEIYz(`noWeO+x#yb_pwj6)R(__%@_Cf>txOQ74wSJ z0#F3(zWWaR-jMEY$7C*3HJrohc79>MCUu26mfYN)f4M~4gD`}EX4e}A!U}QV8!S47 z6y-U-%+h`1n`*pQuKE%Av0@)+wBZr9mH}@vH@i{v(m-6QK7Ncf17x_D=)32`FOjjo zg|^VPf5c6-!FxN{25dvVh#fog=NNpXz zfB$o+0jbRkHH{!TKhE709f+jI^$3#v1Nmf80w`@7-5$1Iv_`)W^px8P-({xwb;D0y z7LKDAHgX<84?l!I*Dvi2#D@oAE^J|g$3!)x1Ua;_;<@#l1fD}lqU2_tS^6Ht$1Wl} zBESo7o^)9-Tjuz$8YQSGhfs{BQV6zW7dA?0b(Dbt=UnQs&4zHfe_sj{RJ4uS-vQpC zX;Bbsuju4%!o8?&m4UZU@~ZZjeFF6ex2ss5_60_JS_|iNc+R0GIjH1@Z z=rLT9%B|WWgOrR7IiIwr2=T;Ne?30M!@{%Qf8o`!>=s<2CBpCK_TWc(DX51>e^xh8 z&@$^b6CgOd7KXQV&Y4%}_#uN*mbanXq(2=Nj`L7H7*k(6F8s6{FOw@(DzU`4-*77{ zF+dxpv}%mFpYK?>N_2*#Y?oB*qEKB}VoQ@bzm>ptmVS_EC(#}Lxxx730trt0G)#$b zE=wVvtqOct1%*9}U{q<)2?{+0TzZzP0jgf9*)arV)*e!f`|jgT{7_9iS@e)recI#z zbzolURQ+TOzE!ymqvBY7+5NnAbWxvMLsLTwEbFqW=CPyCsmJ}P1^V30|D5E|p3BC5 z)3|qgw@ra7aXb-wsa|l^in~1_fm{7bS9jhVRkYVO#U{qMp z)Wce+|DJ}4<2gp8r0_xfZpMo#{Hl2MfjLcZdRB9(B(A(f;+4s*FxV{1F|4d`*sRNd zp4#@sEY|?^FIJ;tmH{@keZ$P(sLh5IdOk@k^0uB^BWr@pk6mHy$qf&~rI>P*a;h0C{%oA*i!VjWn&D~O#MxN&f@1Po# zKN+ zrGrkSjcr?^R#nGl<#Q722^wbYcgW@{+6CBS<1@%dPA8HC!~a`jTz<`g_l5N1M@9wn9GOAZ>nqNgq!yOCbZ@1z`U_N`Z>}+1HIZxk*5RDc&rd5{3qjRh8QmT$VyS;jK z;AF+r6XnnCp=wQYoG|rT2@8&IvKq*IB_WvS%nt%e{MCFm`&W*#LXc|HrD?nVBo=(8*=Aq?u$sDA_sC_RPDUiQ+wnIJET8vx$&fxkW~kP9qXKt zozR)@xGC!P)CTkjeWvXW5&@2?)qt)jiYWWBU?AUtzAN}{JE1I)dfz~7$;}~BmQF`k zpn11qmObXwRB8&rnEG*#4Xax3XBkKlw(;tb?Np^i+H8m(Wyz9k{~ogba@laiEk;2! zV*QV^6g6(QG%vX5Um#^sT&_e`B1pBW5yVth~xUs#0}nv?~C#l?W+9Lsb_5)!71rirGvY zTIJ$OPOY516Y|_014sNv+Z8cc5t_V=i>lWV=vNu#!58y9Zl&GsMEW#pPYPYGHQ|;vFvd*9eM==$_=vc7xnyz0~ zY}r??$<`wAO?JQk@?RGvkWVJlq2dk9vB(yV^vm{=NVI8dhsX<)O(#nr9YD?I?(VmQ z^r7VfUBn<~p3()8yOBjm$#KWx!5hRW)5Jl7wY@ky9lNM^jaT##8QGVsYeaVywmpv>X|Xj7gWE1Ezai&wVLt3p)k4w~yrskT-!PR!kiyQlaxl(( zXhF%Q9x}1TMt3~u@|#wWm-Vq?ZerK={8@~&@9r5JW}r#45#rWii};t`{5#&3$W)|@ zbAf2yDNe0q}NEUvq_Quq3cTjcw z@H_;$hu&xllCI9CFDLuScEMg|x{S7GdV8<&Mq=ezDnRZAyX-8gv97YTm0bg=d)(>N z+B2FcqvI9>jGtnK%eO%y zoBPkJTk%y`8TLf4)IXPBn`U|9>O~WL2C~C$z~9|0m*YH<-vg2CD^SX#&)B4ngOSG$ zV^wmy_iQk>dfN@Pv(ckfy&#ak@MLC7&Q6Ro#!ezM*VEh`+b3Jt%m(^T&p&WJ2Oqvj zs-4nq0TW6cv~(YI$n0UkfwN}kg3_fp?(ijSV#tR9L0}l2qjc7W?i*q01=St0eZ=4h zyGQbEw`9OEH>NMuIe)hVwYHsGERWOD;JxEiO7cQv%pFCeR+IyhwQ|y@&^24k+|8fD zLiOWFNJ2&vu2&`Jv96_z-Cd5RLgmeY3*4rDOQo?Jm`;I_(+ejsPM03!ly!*Cu}Cco zrQSrEDHNyzT(D5s1rZq!8#?f6@v6dB7a-aWs(Qk>N?UGAo{gytlh$%_IhyL7h?DLXDGx zgxGEBQoCAWo-$LRvM=F5MTle`M})t3vVv;2j0HZY&G z22^iGhV@uaJh(XyyY%} zd4iH_UfdV#T=3n}(Lj^|n;O4|$;xhu*8T3hR1mc_A}fK}jfZ7LX~*n5+`8N2q#rI$ z@<_2VANlYF$vIH$ zl<)+*tIWW78IIINA7Rr7i{<;#^yzxoLNkXL)eSs=%|P>$YQIh+ea_3k z_s7r4%j7%&*NHSl?R4k%1>Z=M9o#zxY!n8sL5>BO-ZP;T3Gut>iLS@U%IBrX6BA3k z)&@q}V8a{X<5B}K5s(c(LQ=%v1ocr`t$EqqY0EqVjr65usa=0bkf|O#ky{j3)WBR(((L^wmyHRzoWuL2~WTC=`yZ zn%VX`L=|Ok0v7?s>IHg?yArBcync5rG#^+u)>a%qjES%dRZoIyA8gQ;StH z1Ao7{<&}6U=5}4v<)1T7t!J_CL%U}CKNs-0xWoTTeqj{5{?Be$L0_tk>M9o8 zo371}S#30rKZFM{`H_(L`EM9DGp+Mifk&IP|C2Zu_)Ghr4Qtpmkm1osCf@%Z$%t+7 zYH$Cr)Ro@3-QDeQJ8m+x6%;?YYT;k6Z0E-?kr>x33`H%*ueBD7Zx~3&HtWn0?2Wt} zTG}*|v?{$ajzt}xPzV%lL1t-URi8*Zn)YljXNGDb>;!905Td|mpa@mHjIH%VIiGx- zd@MqhpYFu4_?y5N4xiHn3vX&|e6r~Xt> zZG`aGq|yTNjv;9E+Txuoa@A(9V7g?1_T5FzRI;!=NP1Kqou1z5?%X~Wwb{trRfd>i z8&y^H)8YnKyA_Fyx>}RNmQIczT?w2J4SNvI{5J&}Wto|8FR(W;Qw#b1G<1%#tmYzQ zQ2mZA-PAdi%RQOhkHy9Ea#TPSw?WxwL@H@cbkZwIq0B!@ns}niALidmn&W?!Vd4Gj zO7FiuV4*6Mr^2xlFSvM;Cp_#r8UaqIzHJQg_z^rEJw&OMm_8NGAY2)rKvki|o1bH~ z$2IbfVeY2L(^*rMRU1lM5Y_sgrDS`Z??nR2lX;zyR=c%UyGb*%TC-Dil?SihkjrQy~TMv6;BMs7P8il`H7DmpVm@rJ;b)hW)BL)GjS154b*xq-NXq2cwE z^;VP7ua2pxvCmxrnqUYQMH%a%nHmwmI33nJM(>4LznvY*k&C0{8f*%?zggpDgkuz&JBx{9mfb@wegEl2v!=}Sq2Gaty0<)UrOT0{MZtZ~j5y&w zXlYa_jY)I_+VA-^#mEox#+G>UgvM!Ac8zI<%JRXM_73Q!#i3O|)lOP*qBeJG#BST0 zqohi)O!|$|2SeJQo(w6w7%*92S})XfnhrH_Z8qe!G5>CglP=nI7JAOW?(Z29;pXJ9 zR9`KzQ=WEhy*)WH>$;7Cdz|>*i>=##0bB)oU0OR>>N<21e4rMCHDemNi2LD>Nc$;& zQRFthpWniC1J6@Zh~iJCoLOxN`oCKD5Q4r%ynwgUKPlIEd#?QViIqovY|czyK8>6B zSP%{2-<;%;1`#0mG^B(8KbtXF;Nf>K#Di72UWE4gQ%(_26Koiad)q$xRL~?pN71ZZ zujaaCx~jXjygw;rI!WB=xrOJO6HJ!!w}7eiivtCg5K|F6$EXa)=xUC za^JXSX98W`7g-tm@uo|BKj39Dl;sg5ta;4qjo^pCh~{-HdLl6qI9Ix6f$+qiZ$}s= zNguKrU;u+T@ko(Vr1>)Q%h$?UKXCY>3se%&;h2osl2D zE4A9bd7_|^njDd)6cI*FupHpE3){4NQ*$k*cOWZ_?CZ>Z4_fl@n(mMnYK62Q1d@+I zr&O))G4hMihgBqRIAJkLdk(p(D~X{-oBUA+If@B}j& zsHbeJ3RzTq96lB7d($h$xTeZ^gP0c{t!Y0c)aQE;$FY2!mACg!GDEMKXFOPI^)nHZ z`aSPJpvV0|bbrzhWWkuPURlDeN%VT8tndV8?d)eN*i4I@u zVKl^6{?}A?P)Fsy?3oi#clf}L18t;TjNI2>eI&(ezDK7RyqFxcv%>?oxUlonv(px) z$vnPzRH`y5A(x!yOIfL0bmgeMQB$H5wenx~!ujQK*nUBW;@Em&6Xv2%s(~H5WcU2R z;%Nw<$tI)a`Ve!>x+qegJnQsN2N7HaKzrFqM>`6R*gvh%O*-%THt zrB$Nk;lE;z{s{r^PPm5qz(&lM{sO*g+W{sK+m3M_z=4=&CC>T`{X}1Vg2PEfSj2x_ zmT*(x;ov%3F?qoEeeM>dUn$a*?SIGyO8m806J1W1o+4HRhc2`9$s6hM#qAm zChQ87b~GEw{ADfs+5}FJ8+|bIlIv(jT$Ap#hSHoXdd9#w<#cA<1Rkq^*EEkknUd4& zoIWIY)sAswy6fSERVm&!SO~#iN$OgOX*{9@_BWFyJTvC%S++ilSfCrO(?u=Dc?CXZ zzCG&0yVR{Z`|ZF0eEApWEo#s9osV>F{uK{QA@BES#&;#KsScf>y zvs?vIbI>VrT<*!;XmQS=bhq%46-aambZ(8KU-wOO2=en~D}MCToB_u;Yz{)1ySrPZ z@=$}EvjTdzTWU7c0ZI6L8=yP+YRD_eMMos}b5vY^S*~VZysrkq<`cK3>>v%uy7jgq z0ilW9KjVDHLv0b<1K_`1IkbTOINs0=m-22c%M~l=^S}%hbli-3?BnNq?b`hx^HX2J zIe6ECljRL0uBWb`%{EA=%!i^4sMcj+U_TaTZRb+~GOk z^ZW!nky0n*Wb*r+Q|9H@ml@Z5gU&W`(z4-j!OzC1wOke`TRAYGZVl$PmQ16{3196( zO*?`--I}Qf(2HIwb2&1FB^!faPA2=sLg(@6P4mN)>Dc3i(B0;@O-y2;lM4akD>@^v z=u>*|!s&9zem70g7zfw9FXl1bpJW(C#5w#uy5!V?Q(U35A~$dR%LDVnq@}kQm13{} zd53q3N(s$Eu{R}k2esbftfjfOITCL;jWa$}(mmm}d(&7JZ6d3%IABCapFFYjdEjdK z&4Edqf$G^MNAtL=uCDRs&Fu@FXRgX{*0<(@c3|PNHa>L%zvxWS={L8%qw`STm+=Rd zA}FLspESSIpE_^41~#5yI2bJ=9`oc;GIL!JuW&7YetZ?0H}$$%8rW@*J37L-~Rsx!)8($nI4 zZhcZ2^=Y+p4YPl%j!nFJA|*M^gc(0o$i3nlphe+~-_m}jVkRN{spFs(o0ajW@f3K{ zDV!#BwL322CET$}Y}^0ixYj2w>&Xh12|R8&yEw|wLDvF!lZ#dOTHM9pK6@Nm-@9Lnng4ZHBgBSrr7KI8YCC9DX5Kg|`HsiwJHg2(7#nS;A{b3tVO?Z% za{m5b3rFV6EpX;=;n#wltDv1LE*|g5pQ+OY&*6qCJZc5oDS6Z6JD#6F)bWxZSF@q% z+1WV;m!lRB!n^PC>RgQCI#D1br_o^#iPk>;K2hB~0^<~)?p}LG%kigm@moD#q3PE+ zA^Qca)(xnqw6x>XFhV6ku9r$E>bWNrVH9fum0?4s?Rn2LG{Vm_+QJHse6xa%nzQ?k zKug4PW~#Gtb;#5+9!QBgyB@q=sk9=$S{4T>wjFICStOM?__fr+Kei1 z3j~xPqW;W@YkiUM;HngG!;>@AITg}vAE`M2Pj9Irl4w1fo4w<|Bu!%rh%a(Ai^Zhi zs92>v5;@Y(Zi#RI*ua*h`d_7;byQSa*v9E{2x$<-_=5Z<7{%)}4XExANcz@rK69T0x3%H<@frW>RA8^swA+^a(FxK| zFl3LD*ImHN=XDUkrRhp6RY5$rQ{bRgSO*(vEHYV)3Mo6Jy3puiLmU&g82p{qr0F?ohmbz)f2r{X2|T2 z$4fdQ=>0BeKbiVM!e-lIIs8wVTuC_m7}y4A_%ikI;Wm5$9j(^Y z(cD%U%k)X>_>9~t8;pGzL6L-fmQO@K; zo&vQzMlgY95;1BSkngY)e{`n0!NfVgf}2mB3t}D9@*N;FQ{HZ3Pb%BK6;5#-O|WI( zb6h@qTLU~AbVW#_6?c!?Dj65Now7*pU{h!1+eCV^KCuPAGs28~3k@ueL5+u|Z-7}t z9|lskE`4B7W8wMs@xJa{#bsCGDFoRSNSnmNYB&U7 zVGKWe%+kFB6kb)e;TyHfqtU6~fRg)f|>=5(N36)0+C z`hv65J<$B}WUc!wFAb^QtY31yNleq4dzmG`1wHTj=c*=hay9iD071Hc?oYoUk|M*_ zU1GihAMBsM@5rUJ(qS?9ZYJ6@{bNqJ`2Mr+5#hKf?doa?F|+^IR!8lq9)wS3tF_9n zW_?hm)G(M+MYb?V9YoX^_mu5h-LP^TL^!Q9Z7|@sO(rg_4+@=PdI)WL(B7`!K^ND- z-uIuVDCVEdH_C@c71YGYT^_Scf_dhB8Z2Xy6vGtBSlYud9vggOqv^L~F{BraSE_t} zIkP+Hp2&nH^-MNEs}^`oMLy11`PQW$T|K(`Bu*(f@)mv1-qY(_YG&J2M2<7k;;RK~ zL{Fqj9yCz8(S{}@c)S!65aF<=&eLI{hAMErCx&>i7OeDN>okvegO87OaG{Jmi<|}D zaT@b|0X{d@OIJ7zvT>r+eTzgLq~|Dpu)Z&db-P4z*`M$UL51lf>FLlq6rfG)%doyp z)3kk_YIM!03eQ8Vu_2fg{+osaEJPtJ-s36R+5_AEG12`NG)IQ#TF9c@$99%0iye+ zUzZ57=m2)$D(5Nx!n)=5Au&O0BBgwxIBaeI(mro$#&UGCr<;C{UjJVAbVi%|+WP(a zL$U@TYCxJ=1{Z~}rnW;7UVb7+ZnzgmrogDxhjLGo>c~MiJAWs&&;AGg@%U?Y^0JhL ze(x6Z74JG6FlOFK(T}SXQfhr}RIFl@QXKnIcXYF)5|V~e-}suHILKT-k|<*~Ij|VF zC;t@=uj=hot~*!C68G8hTA%8SzOfETOXQ|3FSaIEjvBJp(A)7SWUi5!Eu#yWgY+;n zlm<$+UDou*V+246_o#V4kMdto8hF%%Lki#zPh}KYXmMf?hrN0;>Mv%`@{0Qn`Ujp) z=lZe+13>^Q!9zT);H<(#bIeRWz%#*}sgUX9P|9($kexOyKIOc`dLux}c$7It4u|Rl z6SSkY*V~g_B-hMPo_ak>>z@AVQ(_N)VY2kB3IZ0G(iDUYw+2d7W^~(Jq}KY=JnWS( z#rzEa&0uNhJ>QE8iiyz;n2H|SV#Og+wEZv=f2%1ELX!SX-(d3tEj$5$1}70Mp<&eI zCkfbByL7af=qQE@5vDVxx1}FSGt_a1DoE3SDI+G)mBAna)KBG4p8Epxl9QZ4BfdAN zFnF|Y(umr;gRgG6NLQ$?ZWgllEeeq~z^ZS7L?<(~O&$5|y)Al^iMKy}&W+eMm1W z7EMU)u^ke(A1#XCV>CZ71}P}0x)4wtHO8#JRG3MA-6g=`ZM!FcICCZ{IEw8Dm2&LQ z1|r)BUG^0GzI6f946RrBlfB1Vs)~8toZf~7)+G;pv&XiUO(%5bm)pl=p>nV^o*;&T z;}@oZSibzto$arQgfkp|z4Z($P>dTXE{4O=vY0!)kDO* zGF8a4wq#VaFpLfK!iELy@?-SeRrdz%F*}hjKcA*y@mj~VD3!it9lhRhX}5YOaR9$} z3mS%$2Be7{l(+MVx3 z(4?h;P!jnRmX9J9sYN#7i=iyj_5q7n#X(!cdqI2lnr8T$IfOW<_v`eB!d9xY1P=2q&WtOXY=D9QYteP)De?S4}FK6#6Ma z=E*V+#s8>L;8aVroK^6iKo=MH{4yEZ_>N-N z`(|;aOATba1^asjxlILk<4}f~`39dBFlxj>Dw(hMYKPO3EEt1@S`1lxFNM+J@uB7T zZ8WKjz7HF1-5&2=l=fqF-*@>n5J}jIxdDwpT?oKM3s8Nr`x8JnN-kCE?~aM1H!hAE z%%w(3kHfGwMnMmNj(SU(w42OrC-euI>Dsjk&jz3ts}WHqmMpzQ3vZrsXrZ|}+MHA7 z068obeXZTsO*6RS@o3x80E4ok``rV^Y3hr&C1;|ZZ0|*EKO`$lECUYG2gVFtUTw)R z4Um<0ZzlON`zTdvVdL#KFoMFQX*a5wM0Czp%wTtfK4Sjs)P**RW&?lP$(<}q%r68Z zS53Y!d@&~ne9O)A^tNrXHhXBkj~$8j%pT1%%mypa9AW5E&s9)rjF4@O3ytH{0z6riz|@< zB~UPh*wRFg2^7EbQrHf0y?E~dHlkOxof_a?M{LqQ^C!i2dawHTPYUE=X@2(3<=OOxs8qn_(y>pU>u^}3y&df{JarR0@VJn0f+U%UiF=$Wyq zQvnVHESil@d|8&R<%}uidGh7@u^(%?$#|&J$pvFC-n8&A>utA=n3#)yMkz+qnG3wd zP7xCnF|$9Dif@N~L)Vde3hW8W!UY0BgT2v(wzp;tlLmyk2%N|0jfG$%<;A&IVrOI< z!L)o>j>;dFaqA3pL}b-Je(bB@VJ4%!JeX@3x!i{yIeIso^=n?fDX`3bU=eG7sTc%g%ye8$v8P@yKE^XD=NYxTb zbf!Mk=h|otpqjFaA-vs5YOF-*GwWPc7VbaOW&stlANnCN8iftFMMrUdYNJ_Bnn5Vt zxfz@Ah|+4&P;reZxp;MmEI7C|FOv8NKUm8njF7Wb6Gi7DeODLl&G~}G4be&*Hi0Qw z5}77vL0P+7-B%UL@3n1&JPxW^d@vVwp?u#gVcJqY9#@-3X{ok#UfW3<1fb%FT`|)V~ggq z(3AUoUS-;7)^hCjdT0Kf{i}h)mBg4qhtHHBti=~h^n^OTH5U*XMgDLIR@sre`AaB$ zg)IGBET_4??m@cx&c~bA80O7B8CHR7(LX7%HThkeC*@vi{-pL%e)yXp!B2InafbDF zjPXf1mko3h59{lT6EEbxKO1Z5GF71)WwowO6kY|6tjSVSWdQ}NsK2x{>i|MKZK8%Q zfu&_0D;CO-Jg0#YmyfctyJ!mRJp)e#@O0mYdp|8x;G1%OZQ3Q847YWTyy|%^cpA;m zze0(5p{tMu^lDkpe?HynyO?a1$_LJl2L&mpeKu%8YvgRNr=%2z${%WThHG=vrWY@4 zsA`OP#O&)TetZ>s%h!=+CE15lOOls&nvC~$Qz0Ph7tHiP;O$i|eDwpT{cp>+)0-|; zY$|bB+Gbel>5aRN3>c0x)4U=|X+z+{ zn*_p*EQoquRL+=+p;=lm`d71&1NqBz&_ph)MXu(Nv6&XE7(RsS)^MGj5Q?Fwude-(sq zjJ>aOq!7!EN>@(fK7EE#;i_BGvli`5U;r!YA{JRodLBc6-`n8K+Fjgwb%sX;j=qHQ z7&Tr!)!{HXoO<2BQrV9Sw?JRaLXV8HrsNevvnf>Y-6|{T!pYLl7jp$-nEE z#X!4G4L#K0qG_4Z;Cj6=;b|Be$hi4JvMH!-voxqx^@8cXp`B??eFBz2lLD8RRaRGh zn7kUfy!YV~p(R|p7iC1Rdgt$_24i0cd-S8HpG|`@my70g^y`gu%#Tf_L21-k?sRRZHK&at(*ED0P8iw{7?R$9~OF$Ko;Iu5)ur5<->x!m93Eb zFYpIx60s=Wxxw=`$aS-O&dCO_9?b1yKiPCQmSQb>T)963`*U+Ydj5kI(B(B?HNP8r z*bfSBpSu)w(Z3j7HQoRjUG(+d=IaE~tv}y14zHHs|0UcN52fT8V_<@2ep_ee{QgZG zmgp8iv4V{k;~8@I%M3<#B;2R>Ef(Gg_cQM7%}0s*^)SK6!Ym+~P^58*wnwV1BW@eG z4sZLqsUvBbFsr#8u7S1r4teQ;t)Y@jnn_m5jS$CsW1um!p&PqAcc8!zyiXHVta9QC zY~wCwCF0U%xiQPD_INKtTb;A|Zf29(mu9NI;E zc-e>*1%(LSXB`g}kd`#}O;veb<(sk~RWL|f3ljxCnEZDdNSTDV6#Td({6l&y4IjKF z^}lIUq*ZUqgTPumD)RrCN{M^jhY>E~1pn|KOZ5((%F)G|*ZQ|r4zIbrEiV%42hJV8 z3xS)=!X1+=olbdGJ=yZil?oXLct8FM{(6ikLL3E%=q#O6(H$p~gQu6T8N!plf!96| z&Q3=`L~>U0zZh;z(pGR2^S^{#PrPxTRHD1RQOON&f)Siaf`GLj#UOk&(|@0?zm;Sx ztsGt8=29-MZs5CSf1l1jNFtNt5rFNZxJPvkNu~2}7*9468TWm>nN9TP&^!;J{-h)_ z7WsHH9|F%I`Pb!>KAS3jQWKfGivTVkMJLO-HUGM_a4UQ_%RgL6WZvrW+Z4ujZn;y@ zz9$=oO!7qVTaQAA^BhX&ZxS*|5dj803M=k&2%QrXda`-Q#IoZL6E(g+tN!6CA!CP* zCpWtCujIea)ENl0liwVfj)Nc<9mV%+e@=d`haoZ*`B7+PNjEbXBkv=B+Pi^~L#EO$D$ZqTiD8f<5$eyb54-(=3 zh)6i8i|jp(@OnRrY5B8t|LFXFQVQ895n*P16cEKTrT*~yLH6Z4e*bZ5otpRDri&+A zfNbK1D5@O=sm`fN=WzWyse!za5n%^+6dHPGX#8DyIK>?9qyX}2XvBWVqbP%%D)7$= z=#$WulZlZR<{m#gU7lwqK4WS1Ne$#_P{b17qe$~UOXCl>5b|6WVh;5vVnR<%d+Lnp z$uEmML38}U4vaW8>shm6CzB(Wei3s#NAWE3)a2)z@i{4jTn;;aQS)O@l{rUM`J@K& l00vQ5JBs~;vo!vr%%-k{2_Fq1Mn4QF81S)AQ99zk{{c4yR+0b! literal 59536 zcma&NbC71ylI~qywr$(CZQJHswz}-9F59+k+g;UV+cs{`J?GrGXYR~=-ydruB3JCa zB64N^cILAcWk5iofq)<(fq;O7{th4@;QxID0)qN`mJ?GIqLY#rX8-|G{5M0pdVW5^ zzXk$-2kQTAC?_N@B`&6-N-rmVFE=$QD?>*=4<|!MJu@}isLc4AW#{m2if&A5T5g&~ ziuMQeS*U5sL6J698wOd)K@oK@1{peP5&Esut<#VH^u)gp`9H4)`uE!2$>RTctN+^u z=ASkePDZA-X8)rp%D;p*~P?*a_=*Kwc<^>QSH|^<0>o37lt^+Mj1;4YvJ(JR-Y+?%Nu}JAYj5 z_Qc5%Ao#F?q32i?ZaN2OSNhWL;2oDEw_({7ZbgUjna!Fqn3NzLM@-EWFPZVmc>(fZ z0&bF-Ch#p9C{YJT9Rcr3+Y_uR^At1^BxZ#eo>$PLJF3=;t_$2|t+_6gg5(j{TmjYU zK12c&lE?Eh+2u2&6Gf*IdKS&6?rYbSEKBN!rv{YCm|Rt=UlPcW9j`0o6{66#y5t9C zruFA2iKd=H%jHf%ypOkxLnO8#H}#Zt{8p!oi6)7#NqoF({t6|J^?1e*oxqng9Q2Cc zg%5Vu!em)}Yuj?kaP!D?b?(C*w!1;>R=j90+RTkyEXz+9CufZ$C^umX^+4|JYaO<5 zmIM3#dv`DGM;@F6;(t!WngZSYzHx?9&$xEF70D1BvfVj<%+b#)vz)2iLCrTeYzUcL z(OBnNoG6Le%M+@2oo)&jdOg=iCszzv59e zDRCeaX8l1hC=8LbBt|k5?CXgep=3r9BXx1uR8!p%Z|0+4Xro=xi0G!e{c4U~1j6!) zH6adq0}#l{%*1U(Cb%4AJ}VLWKBPi0MoKFaQH6x?^hQ!6em@993xdtS%_dmevzeNl z(o?YlOI=jl(`L9^ z0O+H9k$_@`6L13eTT8ci-V0ljDMD|0ifUw|Q-Hep$xYj0hTO@0%IS^TD4b4n6EKDG z??uM;MEx`s98KYN(K0>c!C3HZdZ{+_53DO%9k5W%pr6yJusQAv_;IA}925Y%;+!tY z%2k!YQmLLOr{rF~!s<3-WEUs)`ix_mSU|cNRBIWxOox_Yb7Z=~Q45ZNe*u|m^|)d* zog=i>`=bTe!|;8F+#H>EjIMcgWcG2ORD`w0WD;YZAy5#s{65~qfI6o$+Ty&-hyMyJ z3Ra~t>R!p=5ZpxA;QkDAoPi4sYOP6>LT+}{xp}tk+<0k^CKCFdNYG(Es>p0gqD)jP zWOeX5G;9(m@?GOG7g;e74i_|SmE?`B2i;sLYwRWKLy0RLW!Hx`=!LH3&k=FuCsM=9M4|GqzA)anEHfxkB z?2iK-u(DC_T1};KaUT@3nP~LEcENT^UgPvp!QC@Dw&PVAhaEYrPey{nkcn(ro|r7XUz z%#(=$7D8uP_uU-oPHhd>>^adbCSQetgSG`e$U|7mr!`|bU0aHl_cmL)na-5x1#OsVE#m*+k84Y^+UMeSAa zbrVZHU=mFwXEaGHtXQq`2ZtjfS!B2H{5A<3(nb-6ARVV8kEmOkx6D2x7~-6hl;*-*}2Xz;J#a8Wn;_B5=m zl3dY;%krf?i-Ok^Pal-}4F`{F@TYPTwTEhxpZK5WCpfD^UmM_iYPe}wpE!Djai6_{ z*pGO=WB47#Xjb7!n2Ma)s^yeR*1rTxp`Mt4sfA+`HwZf%!7ZqGosPkw69`Ix5Ku6G z@Pa;pjzV&dn{M=QDx89t?p?d9gna*}jBly*#1!6}5K<*xDPJ{wv4& zM$17DFd~L*Te3A%yD;Dp9UGWTjRxAvMu!j^Tbc}2v~q^59d4bz zvu#!IJCy(BcWTc`;v$9tH;J%oiSJ_i7s;2`JXZF+qd4C)vY!hyCtl)sJIC{ebI*0> z@x>;EzyBv>AI-~{D6l6{ST=em*U( z(r$nuXY-#CCi^8Z2#v#UXOt`dbYN1z5jzNF2 z411?w)whZrfA20;nl&C1Gi+gk<`JSm+{|*2o<< zqM#@z_D`Cn|0H^9$|Tah)0M_X4c37|KQ*PmoT@%xHc3L1ZY6(p(sNXHa&49Frzto& zR`c~ClHpE~4Z=uKa5S(-?M8EJ$zt0&fJk~p$M#fGN1-y$7!37hld`Uw>Urri(DxLa;=#rK0g4J)pXMC zxzraOVw1+kNWpi#P=6(qxf`zSdUC?D$i`8ZI@F>k6k zz21?d+dw7b&i*>Kv5L(LH-?J%@WnqT7j#qZ9B>|Zl+=> z^U-pV@1y_ptHo4hl^cPRWewbLQ#g6XYQ@EkiP z;(=SU!yhjHp%1&MsU`FV1Z_#K1&(|5n(7IHbx&gG28HNT)*~-BQi372@|->2Aw5It z0CBpUcMA*QvsPy)#lr!lIdCi@1k4V2m!NH)%Px(vu-r(Q)HYc!p zJ^$|)j^E#q#QOgcb^pd74^JUi7fUmMiNP_o*lvx*q%_odv49Dsv$NV;6J z9GOXKomA{2Pb{w}&+yHtH?IkJJu~}Z?{Uk++2mB8zyvh*xhHKE``99>y#TdD z&(MH^^JHf;g(Tbb^&8P*;_i*2&fS$7${3WJtV7K&&(MBV2~)2KB3%cWg#1!VE~k#C z!;A;?p$s{ihyojEZz+$I1)L}&G~ml=udD9qh>Tu(ylv)?YcJT3ihapi!zgPtWb*CP zlLLJSRCj-^w?@;RU9aL2zDZY1`I3d<&OMuW=c3$o0#STpv_p3b9Wtbql>w^bBi~u4 z3D8KyF?YE?=HcKk!xcp@Cigvzy=lnFgc^9c%(^F22BWYNAYRSho@~*~S)4%AhEttv zvq>7X!!EWKG?mOd9&n>vvH1p4VzE?HCuxT-u+F&mnsfDI^}*-d00-KAauEaXqg3k@ zy#)MGX!X;&3&0s}F3q40ZmVM$(H3CLfpdL?hB6nVqMxX)q=1b}o_PG%r~hZ4gUfSp zOH4qlEOW4OMUc)_m)fMR_rl^pCfXc{$fQbI*E&mV77}kRF z&{<06AJyJ!e863o-V>FA1a9Eemx6>^F$~9ppt()ZbPGfg_NdRXBWoZnDy2;#ODgf! zgl?iOcF7Meo|{AF>KDwTgYrJLb$L2%%BEtO>T$C?|9bAB&}s;gI?lY#^tttY&hfr# zKhC+&b-rpg_?~uVK%S@mQleU#_xCsvIPK*<`E0fHE1&!J7!xD#IB|SSPW6-PyuqGn3^M^Rz%WT{e?OI^svARX&SAdU77V(C~ zM$H{Kg59op{<|8ry9ecfP%=kFm(-!W&?U0@<%z*+!*<e0XesMxRFu9QnGqun6R_%T+B%&9Dtk?*d$Q zb~>84jEAPi@&F@3wAa^Lzc(AJz5gsfZ7J53;@D<;Klpl?sK&u@gie`~vTsbOE~Cd4 z%kr56mI|#b(Jk&;p6plVwmNB0H@0SmgdmjIn5Ne@)}7Vty(yb2t3ev@22AE^s!KaN zyQ>j+F3w=wnx7w@FVCRe+`vUH)3gW%_72fxzqX!S&!dchdkRiHbXW1FMrIIBwjsai8`CB2r4mAbwp%rrO>3B$Zw;9=%fXI9B{d(UzVap7u z6piC-FQ)>}VOEuPpuqznpY`hN4dGa_1Xz9rVg(;H$5Te^F0dDv*gz9JS<|>>U0J^# z6)(4ICh+N_Q`Ft0hF|3fSHs*?a=XC;e`sJaU9&d>X4l?1W=|fr!5ShD|nv$GK;j46@BV6+{oRbWfqOBRb!ir88XD*SbC(LF}I1h#6@dvK%Toe%@ zhDyG$93H8Eu&gCYddP58iF3oQH*zLbNI;rN@E{T9%A8!=v#JLxKyUe}e}BJpB{~uN zqgxRgo0*-@-iaHPV8bTOH(rS(huwK1Xg0u+e!`(Irzu@Bld&s5&bWgVc@m7;JgELd zimVs`>vQ}B_1(2#rv#N9O`fJpVfPc7V2nv34PC);Dzbb;p!6pqHzvy?2pD&1NE)?A zt(t-ucqy@wn9`^MN5apa7K|L=9>ISC>xoc#>{@e}m#YAAa1*8-RUMKwbm|;5p>T`Z zNf*ph@tnF{gmDa3uwwN(g=`Rh)4!&)^oOy@VJaK4lMT&5#YbXkl`q?<*XtsqD z9PRK6bqb)fJw0g-^a@nu`^?71k|m3RPRjt;pIkCo1{*pdqbVs-Yl>4E>3fZx3Sv44grW=*qdSoiZ9?X0wWyO4`yDHh2E!9I!ZFi zVL8|VtW38}BOJHW(Ax#KL_KQzarbuE{(%TA)AY)@tY4%A%P%SqIU~8~-Lp3qY;U-} z`h_Gel7;K1h}7$_5ZZT0&%$Lxxr-<89V&&TCsu}LL#!xpQ1O31jaa{U34~^le*Y%L za?7$>Jk^k^pS^_M&cDs}NgXlR>16AHkSK-4TRaJSh#h&p!-!vQY%f+bmn6x`4fwTp z$727L^y`~!exvmE^W&#@uY!NxJi`g!i#(++!)?iJ(1)2Wk;RN zFK&O4eTkP$Xn~4bB|q8y(btx$R#D`O@epi4ofcETrx!IM(kWNEe42Qh(8*KqfP(c0 zouBl6>Fc_zM+V;F3znbo{x#%!?mH3`_ANJ?y7ppxS@glg#S9^MXu|FM&ynpz3o&Qh z2ujAHLF3($pH}0jXQsa#?t--TnF1P73b?4`KeJ9^qK-USHE)4!IYgMn-7z|=ALF5SNGkrtPG@Y~niUQV2?g$vzJN3nZ{7;HZHzWAeQ;5P|@Tl3YHpyznGG4-f4=XflwSJY+58-+wf?~Fg@1p1wkzuu-RF3j2JX37SQUc? zQ4v%`V8z9ZVZVqS8h|@@RpD?n0W<=hk=3Cf8R?d^9YK&e9ZybFY%jdnA)PeHvtBe- zhMLD+SSteHBq*q)d6x{)s1UrsO!byyLS$58WK;sqip$Mk{l)Y(_6hEIBsIjCr5t>( z7CdKUrJTrW%qZ#1z^n*Lb8#VdfzPw~OIL76aC+Rhr<~;4Tl!sw?Rj6hXj4XWa#6Tp z@)kJ~qOV)^Rh*-?aG>ic2*NlC2M7&LUzc9RT6WM%Cpe78`iAowe!>(T0jo&ivn8-7 zs{Qa@cGy$rE-3AY0V(l8wjI^uB8Lchj@?L}fYal^>T9z;8juH@?rG&g-t+R2dVDBe zq!K%{e-rT5jX19`(bP23LUN4+_zh2KD~EAYzhpEO3MUG8@}uBHH@4J zd`>_(K4q&>*k82(dDuC)X6JuPrBBubOg7qZ{?x!r@{%0);*`h*^F|%o?&1wX?Wr4b z1~&cy#PUuES{C#xJ84!z<1tp9sfrR(i%Tu^jnXy;4`Xk;AQCdFC@?V%|; zySdC7qS|uQRcH}EFZH%mMB~7gi}a0utE}ZE_}8PQH8f;H%PN41Cb9R%w5Oi5el^fd z$n{3SqLCnrF##x?4sa^r!O$7NX!}&}V;0ZGQ&K&i%6$3C_dR%I7%gdQ;KT6YZiQrW zk%q<74oVBV>@}CvJ4Wj!d^?#Zwq(b$E1ze4$99DuNg?6t9H}k_|D7KWD7i0-g*EO7 z;5{hSIYE4DMOK3H%|f5Edx+S0VI0Yw!tsaRS2&Il2)ea^8R5TG72BrJue|f_{2UHa z@w;^c|K3da#$TB0P3;MPlF7RuQeXT$ zS<<|C0OF(k)>fr&wOB=gP8!Qm>F41u;3esv7_0l%QHt(~+n; zf!G6%hp;Gfa9L9=AceiZs~tK+Tf*Wof=4!u{nIO90jH@iS0l+#%8=~%ASzFv7zqSB^?!@N7)kp0t&tCGLmzXSRMRyxCmCYUD2!B`? zhs$4%KO~m=VFk3Buv9osha{v+mAEq=ik3RdK@;WWTV_g&-$U4IM{1IhGX{pAu%Z&H zFfwCpUsX%RKg);B@7OUzZ{Hn{q6Vv!3#8fAg!P$IEx<0vAx;GU%}0{VIsmFBPq_mb zpe^BChDK>sc-WLKl<6 zwbW|e&d&dv9Wu0goueyu>(JyPx1mz0v4E?cJjFuKF71Q1)AL8jHO$!fYT3(;U3Re* zPPOe%*O+@JYt1bW`!W_1!mN&=w3G9ru1XsmwfS~BJ))PhD(+_J_^N6j)sx5VwbWK| zwRyC?W<`pOCY)b#AS?rluxuuGf-AJ=D!M36l{ua?@SJ5>e!IBr3CXIxWw5xUZ@Xrw z_R@%?{>d%Ld4p}nEsiA@v*nc6Ah!MUs?GA7e5Q5lPpp0@`%5xY$C;{%rz24$;vR#* zBP=a{)K#CwIY%p} zXVdxTQ^HS@O&~eIftU+Qt^~(DGxrdi3k}DdT^I7Iy5SMOp$QuD8s;+93YQ!OY{eB24%xY7ml@|M7I(Nb@K_-?F;2?et|CKkuZK_>+>Lvg!>JE~wN`BI|_h6$qi!P)+K-1Hh(1;a`os z55)4Q{oJiA(lQM#;w#Ta%T0jDNXIPM_bgESMCDEg6rM33anEr}=|Fn6)|jBP6Y}u{ zv9@%7*#RI9;fv;Yii5CI+KrRdr0DKh=L>)eO4q$1zmcSmglsV`*N(x=&Wx`*v!!hn6X-l0 zP_m;X??O(skcj+oS$cIdKhfT%ABAzz3w^la-Ucw?yBPEC+=Pe_vU8nd-HV5YX6X8r zZih&j^eLU=%*;VzhUyoLF;#8QsEfmByk+Y~caBqSvQaaWf2a{JKB9B>V&r?l^rXaC z8)6AdR@Qy_BxQrE2Fk?ewD!SwLuMj@&d_n5RZFf7=>O>hzVE*seW3U?_p|R^CfoY`?|#x9)-*yjv#lo&zP=uI`M?J zbzC<^3x7GfXA4{FZ72{PE*-mNHyy59Q;kYG@BB~NhTd6pm2Oj=_ zizmD?MKVRkT^KmXuhsk?eRQllPo2Ubk=uCKiZ&u3Xjj~<(!M94c)Tez@9M1Gfs5JV z->@II)CDJOXTtPrQudNjE}Eltbjq>6KiwAwqvAKd^|g!exgLG3;wP+#mZYr`cy3#39e653d=jrR-ulW|h#ddHu(m9mFoW~2yE zz5?dB%6vF}+`-&-W8vy^OCxm3_{02royjvmwjlp+eQDzFVEUiyO#gLv%QdDSI#3W* z?3!lL8clTaNo-DVJw@ynq?q!%6hTQi35&^>P85G$TqNt78%9_sSJt2RThO|JzM$iL zg|wjxdMC2|Icc5rX*qPL(coL!u>-xxz-rFiC!6hD1IR%|HSRsV3>Kq~&vJ=s3M5y8SG%YBQ|{^l#LGlg!D?E>2yR*eV%9m$_J6VGQ~AIh&P$_aFbh zULr0Z$QE!QpkP=aAeR4ny<#3Fwyw@rZf4?Ewq`;mCVv}xaz+3ni+}a=k~P+yaWt^L z@w67!DqVf7D%7XtXX5xBW;Co|HvQ8WR1k?r2cZD%U;2$bsM%u8{JUJ5Z0k= zZJARv^vFkmWx15CB=rb=D4${+#DVqy5$C%bf`!T0+epLJLnh1jwCdb*zuCL}eEFvE z{rO1%gxg>1!W(I!owu*mJZ0@6FM(?C+d*CeceZRW_4id*D9p5nzMY&{mWqrJomjIZ z97ZNnZ3_%Hx8dn;H>p8m7F#^2;T%yZ3H;a&N7tm=Lvs&lgJLW{V1@h&6Vy~!+Ffbb zv(n3+v)_D$}dqd!2>Y2B)#<+o}LH#%ogGi2-?xRIH)1!SD)u-L65B&bsJTC=LiaF+YOCif2dUX6uAA|#+vNR z>U+KQekVGon)Yi<93(d!(yw1h3&X0N(PxN2{%vn}cnV?rYw z$N^}_o!XUB!mckL`yO1rnUaI4wrOeQ(+&k?2mi47hzxSD`N#-byqd1IhEoh!PGq>t z_MRy{5B0eKY>;Ao3z$RUU7U+i?iX^&r739F)itdrTpAi-NN0=?^m%?{A9Ly2pVv>Lqs6moTP?T2-AHqFD-o_ znVr|7OAS#AEH}h8SRPQ@NGG47dO}l=t07__+iK8nHw^(AHx&Wb<%jPc$$jl6_p(b$ z)!pi(0fQodCHfM)KMEMUR&UID>}m^(!{C^U7sBDOA)$VThRCI0_+2=( zV8mMq0R(#z;C|7$m>$>`tX+T|xGt(+Y48@ZYu#z;0pCgYgmMVbFb!$?%yhZqP_nhn zy4<#3P1oQ#2b51NU1mGnHP$cf0j-YOgAA}A$QoL6JVLcmExs(kU{4z;PBHJD%_=0F z>+sQV`mzijSIT7xn%PiDKHOujX;n|M&qr1T@rOxTdxtZ!&u&3HHFLYD5$RLQ=heur zb>+AFokUVQeJy-#LP*^)spt{mb@Mqe=A~-4p0b+Bt|pZ+@CY+%x}9f}izU5;4&QFE zO1bhg&A4uC1)Zb67kuowWY4xbo&J=%yoXlFB)&$d*-}kjBu|w!^zbD1YPc0-#XTJr z)pm2RDy%J3jlqSMq|o%xGS$bPwn4AqitC6&e?pqWcjWPt{3I{>CBy;hg0Umh#c;hU3RhCUX=8aR>rmd` z7Orw(5tcM{|-^J?ZAA9KP|)X6n9$-kvr#j5YDecTM6n z&07(nD^qb8hpF0B^z^pQ*%5ePYkv&FabrlI61ntiVp!!C8y^}|<2xgAd#FY=8b*y( zuQOuvy2`Ii^`VBNJB&R!0{hABYX55ooCAJSSevl4RPqEGb)iy_0H}v@vFwFzD%>#I>)3PsouQ+_Kkbqy*kKdHdfkN7NBcq%V{x^fSxgXpg7$bF& zj!6AQbDY(1u#1_A#1UO9AxiZaCVN2F0wGXdY*g@x$ByvUA?ePdide0dmr#}udE%K| z3*k}Vv2Ew2u1FXBaVA6aerI36R&rzEZeDDCl5!t0J=ug6kuNZzH>3i_VN`%BsaVB3 zQYw|Xub_SGf{)F{$ZX5`Jc!X!;eybjP+o$I{Z^Hsj@D=E{MnnL+TbC@HEU2DjG{3-LDGIbq()U87x4eS;JXnSh;lRlJ z>EL3D>wHt-+wTjQF$fGyDO$>d+(fq@bPpLBS~xA~R=3JPbS{tzN(u~m#Po!?H;IYv zE;?8%^vle|%#oux(Lj!YzBKv+Fd}*Ur-dCBoX*t{KeNM*n~ZPYJ4NNKkI^MFbz9!v z4(Bvm*Kc!-$%VFEewYJKz-CQN{`2}KX4*CeJEs+Q(!kI%hN1!1P6iOq?ovz}X0IOi z)YfWpwW@pK08^69#wSyCZkX9?uZD?C^@rw^Y?gLS_xmFKkooyx$*^5#cPqntNTtSG zlP>XLMj2!VF^0k#ole7`-c~*~+_T5ls?x4)ah(j8vo_ zwb%S8qoaZqY0-$ZI+ViIA_1~~rAH7K_+yFS{0rT@eQtTAdz#8E5VpwnW!zJ_^{Utv zlW5Iar3V5t&H4D6A=>?mq;G92;1cg9a2sf;gY9pJDVKn$DYdQlvfXq}zz8#LyPGq@ z+`YUMD;^-6w&r-82JL7mA8&M~Pj@aK!m{0+^v<|t%APYf7`}jGEhdYLqsHW-Le9TL z_hZZ1gbrz7$f9^fAzVIP30^KIz!!#+DRLL+qMszvI_BpOSmjtl$hh;&UeM{ER@INV zcI}VbiVTPoN|iSna@=7XkP&-4#06C};8ajbxJ4Gcq8(vWv4*&X8bM^T$mBk75Q92j z1v&%a;OSKc8EIrodmIiw$lOES2hzGDcjjB`kEDfJe{r}yE6`eZL zEB`9u>Cl0IsQ+t}`-cx}{6jqcANucqIB>Qmga_&<+80E2Q|VHHQ$YlAt{6`Qu`HA3 z03s0-sSlwbvgi&_R8s={6<~M^pGvBNjKOa>tWenzS8s zR>L7R5aZ=mSU{f?ib4Grx$AeFvtO5N|D>9#)ChH#Fny2maHWHOf2G=#<9Myot#+4u zWVa6d^Vseq_0=#AYS(-m$Lp;*8nC_6jXIjEM`omUmtH@QDs3|G)i4j*#_?#UYVZvJ z?YjT-?!4Q{BNun;dKBWLEw2C-VeAz`%?A>p;)PL}TAZn5j~HK>v1W&anteARlE+~+ zj>c(F;?qO3pXBb|#OZdQnm<4xWmn~;DR5SDMxt0UK_F^&eD|KZ=O;tO3vy4@4h^;2 zUL~-z`-P1aOe?|ZC1BgVsL)2^J-&vIFI%q@40w0{jjEfeVl)i9(~bt2z#2Vm)p`V_ z1;6$Ae7=YXk#=Qkd24Y23t&GvRxaOoad~NbJ+6pxqzJ>FY#Td7@`N5xp!n(c!=RE& z&<<@^a$_Ys8jqz4|5Nk#FY$~|FPC0`*a5HH!|Gssa9=~66&xG9)|=pOOJ2KE5|YrR zw!w6K2aC=J$t?L-;}5hn6mHd%hC;p8P|Dgh6D>hGnXPgi;6r+eA=?f72y9(Cf_ho{ zH6#)uD&R=73^$$NE;5piWX2bzR67fQ)`b=85o0eOLGI4c-Tb@-KNi2pz=Ke@SDcPn za$AxXib84`!Sf;Z3B@TSo`Dz7GM5Kf(@PR>Ghzi=BBxK8wRp>YQoXm+iL>H*Jo9M3 z6w&E?BC8AFTFT&Tv8zf+m9<&S&%dIaZ)Aoqkak_$r-2{$d~0g2oLETx9Y`eOAf14QXEQw3tJne;fdzl@wV#TFXSLXM2428F-Q}t+n2g%vPRMUzYPvzQ9f# zu(liiJem9P*?0%V@RwA7F53r~|I!Ty)<*AsMX3J{_4&}{6pT%Tpw>)^|DJ)>gpS~1rNEh z0$D?uO8mG?H;2BwM5a*26^7YO$XjUm40XmBsb63MoR;bJh63J;OngS5sSI+o2HA;W zdZV#8pDpC9Oez&L8loZO)MClRz!_!WD&QRtQxnazhT%Vj6Wl4G11nUk8*vSeVab@N#oJ}`KyJv+8Mo@T1-pqZ1t|?cnaVOd;1(h9 z!$DrN=jcGsVYE-0-n?oCJ^4x)F}E;UaD-LZUIzcD?W^ficqJWM%QLy6QikrM1aKZC zi{?;oKwq^Vsr|&`i{jIphA8S6G4)$KGvpULjH%9u(Dq247;R#l&I0{IhcC|oBF*Al zvLo7Xte=C{aIt*otJD}BUq)|_pdR>{zBMT< z(^1RpZv*l*m*OV^8>9&asGBo8h*_4q*)-eCv*|Pq=XNGrZE)^(SF7^{QE_~4VDB(o zVcPA_!G+2CAtLbl+`=Q~9iW`4ZRLku!uB?;tWqVjB0lEOf}2RD7dJ=BExy=<9wkb- z9&7{XFA%n#JsHYN8t5d~=T~5DcW4$B%3M+nNvC2`0!#@sckqlzo5;hhGi(D9=*A4` z5ynobawSPRtWn&CDLEs3Xf`(8^zDP=NdF~F^s&={l7(aw&EG}KWpMjtmz7j_VLO;@ zM2NVLDxZ@GIv7*gzl1 zjq78tv*8#WSY`}Su0&C;2F$Ze(q>F(@Wm^Gw!)(j;dk9Ad{STaxn)IV9FZhm*n+U} zi;4y*3v%A`_c7a__DJ8D1b@dl0Std3F||4Wtvi)fCcBRh!X9$1x!_VzUh>*S5s!oq z;qd{J_r79EL2wIeiGAqFstWtkfIJpjVh%zFo*=55B9Zq~y0=^iqHWfQl@O!Ak;(o*m!pZqe9 z%U2oDOhR)BvW8&F70L;2TpkzIutIvNQaTjjs5V#8mV4!NQ}zN=i`i@WI1z0eN-iCS z;vL-Wxc^Vc_qK<5RPh(}*8dLT{~GzE{w2o$2kMFaEl&q zP{V=>&3kW7tWaK-Exy{~`v4J0U#OZBk{a9{&)&QG18L@6=bsZ1zC_d{{pKZ-Ey>I> z;8H0t4bwyQqgu4hmO`3|4K{R*5>qnQ&gOfdy?z`XD%e5+pTDzUt3`k^u~SaL&XMe= z9*h#kT(*Q9jO#w2Hd|Mr-%DV8i_1{J1MU~XJ3!WUplhXDYBpJH><0OU`**nIvPIof z|N8@I=wA)sf45SAvx||f?Z5uB$kz1qL3Ky_{%RPdP5iN-D2!p5scq}buuC00C@jom zhfGKm3|f?Z0iQ|K$Z~!`8{nmAS1r+fp6r#YDOS8V*;K&Gs7Lc&f^$RC66O|)28oh`NHy&vq zJh+hAw8+ybTB0@VhWN^0iiTnLsCWbS_y`^gs!LX!Lw{yE``!UVzrV24tP8o;I6-65 z1MUiHw^{bB15tmrVT*7-#sj6cs~z`wk52YQJ*TG{SE;KTm#Hf#a~|<(|ImHH17nNM z`Ub{+J3dMD!)mzC8b(2tZtokKW5pAwHa?NFiso~# z1*iaNh4lQ4TS)|@G)H4dZV@l*Vd;Rw;-;odDhW2&lJ%m@jz+Panv7LQm~2Js6rOW3 z0_&2cW^b^MYW3)@o;neZ<{B4c#m48dAl$GCc=$>ErDe|?y@z`$uq3xd(%aAsX)D%l z>y*SQ%My`yDP*zof|3@_w#cjaW_YW4BdA;#Glg1RQcJGY*CJ9`H{@|D+*e~*457kd z73p<%fB^PV!Ybw@)Dr%(ZJbX}xmCStCYv#K3O32ej{$9IzM^I{6FJ8!(=azt7RWf4 z7ib0UOPqN40X!wOnFOoddd8`!_IN~9O)#HRTyjfc#&MCZ zZAMzOVB=;qwt8gV?{Y2?b=iSZG~RF~uyx18K)IDFLl})G1v@$(s{O4@RJ%OTJyF+Cpcx4jmy|F3euCnMK!P2WTDu5j z{{gD$=M*pH!GGzL%P)V2*ROm>!$Y=z|D`!_yY6e7SU$~a5q8?hZGgaYqaiLnkK%?0 zs#oI%;zOxF@g*@(V4p!$7dS1rOr6GVs6uYCTt2h)eB4?(&w8{#o)s#%gN@BBosRUe z)@P@8_Zm89pr~)b>e{tbPC~&_MR--iB{=)y;INU5#)@Gix-YpgP<-c2Ms{9zuCX|3 z!p(?VaXww&(w&uBHzoT%!A2=3HAP>SDxcljrego7rY|%hxy3XlODWffO_%g|l+7Y_ zqV(xbu)s4lV=l7M;f>vJl{`6qBm>#ZeMA}kXb97Z)?R97EkoI?x6Lp0yu1Z>PS?2{ z0QQ(8D)|lc9CO3B~e(pQM&5(1y&y=e>C^X$`)_&XuaI!IgDTVqt31wX#n+@!a_A0ZQkA zCJ2@M_4Gb5MfCrm5UPggeyh)8 zO9?`B0J#rkoCx(R0I!ko_2?iO@|oRf1;3r+i)w-2&j?=;NVIdPFsB)`|IC0zk6r9c zRrkfxWsiJ(#8QndNJj@{@WP2Ackr|r1VxV{7S&rSU(^)-M8gV>@UzOLXu9K<{6e{T zXJ6b92r$!|lwjhmgqkdswY&}c)KW4A)-ac%sU;2^fvq7gfUW4Bw$b!i@duy1CAxSn z(pyh$^Z=&O-q<{bZUP+$U}=*#M9uVc>CQVgDs4swy5&8RAHZ~$)hrTF4W zPsSa~qYv_0mJnF89RnnJTH`3}w4?~epFl=D(35$ zWa07ON$`OMBOHgCmfO(9RFc<)?$x)N}Jd2A(<*Ll7+4jrRt9w zwGxExUXd9VB#I|DwfxvJ;HZ8Q{37^wDhaZ%O!oO(HpcqfLH%#a#!~;Jl7F5>EX_=8 z{()l2NqPz>La3qJR;_v+wlK>GsHl;uRA8%j`A|yH@k5r%55S9{*Cp%uw6t`qc1!*T za2OeqtQj7sAp#Q~=5Fs&aCR9v>5V+s&RdNvo&H~6FJOjvaj--2sYYBvMq;55%z8^o z|BJDA4vzfow#DO#ZQHh;Oq_{r+qP{R9ox2TOgwQiv7Ow!zjN+A@BN;0tA2lUb#+zO z(^b89eV)D7UVE+h{mcNc6&GtpOqDn_?VAQ)Vob$hlFwW%xh>D#wml{t&Ofmm_d_+; zKDxzdr}`n2Rw`DtyIjrG)eD0vut$}dJAZ0AohZ+ZQdWXn_Z@dI_y=7t3q8x#pDI-K z2VVc&EGq445Rq-j0=U=Zx`oBaBjsefY;%)Co>J3v4l8V(T8H?49_@;K6q#r~Wwppc z4XW0(4k}cP=5ex>-Xt3oATZ~bBWKv)aw|I|Lx=9C1s~&b77idz({&q3T(Y(KbWO?+ zmcZ6?WeUsGk6>km*~234YC+2e6Zxdl~<_g2J|IE`GH%n<%PRv-50; zH{tnVts*S5*_RxFT9eM0z-pksIb^drUq4>QSww=u;UFCv2AhOuXE*V4z?MM`|ABOC4P;OfhS(M{1|c%QZ=!%rQTDFx`+}?Kdx$&FU?Y<$x;j7z=(;Lyz+?EE>ov!8vvMtSzG!nMie zsBa9t8as#2nH}n8xzN%W%U$#MHNXmDUVr@GX{?(=yI=4vks|V)!-W5jHsU|h_&+kY zS_8^kd3jlYqOoiI`ZqBVY!(UfnAGny!FowZWY_@YR0z!nG7m{{)4OS$q&YDyw6vC$ zm4!$h>*|!2LbMbxS+VM6&DIrL*X4DeMO!@#EzMVfr)e4Tagn~AQHIU8?e61TuhcKD zr!F4(kEebk(Wdk-?4oXM(rJwanS>Jc%<>R(siF+>+5*CqJLecP_we33iTFTXr6W^G z7M?LPC-qFHK;E!fxCP)`8rkxZyFk{EV;G-|kwf4b$c1k0atD?85+|4V%YATWMG|?K zLyLrws36p%Qz6{}>7b>)$pe>mR+=IWuGrX{3ZPZXF3plvuv5Huax86}KX*lbPVr}L z{C#lDjdDeHr~?l|)Vp_}T|%$qF&q#U;ClHEPVuS+Jg~NjC1RP=17=aQKGOcJ6B3mp z8?4*-fAD~}sX*=E6!}^u8)+m2j<&FSW%pYr_d|p_{28DZ#Cz0@NF=gC-o$MY?8Ca8 zr5Y8DSR^*urS~rhpX^05r30Ik#2>*dIOGxRm0#0YX@YQ%Mg5b6dXlS!4{7O_kdaW8PFSdj1=ryI-=5$fiieGK{LZ+SX(1b=MNL!q#lN zv98?fqqTUH8r8C7v(cx#BQ5P9W>- zmW93;eH6T`vuJ~rqtIBg%A6>q>gnWb3X!r0wh_q;211+Om&?nvYzL1hhtjB zK_7G3!n7PL>d!kj){HQE zE8(%J%dWLh1_k%gVXTZt zEdT09XSKAx27Ncaq|(vzL3gm83q>6CAw<$fTnMU05*xAe&rDfCiu`u^1)CD<>sx0i z*hr^N_TeN89G(nunZoLBf^81#pmM}>JgD@Nn1l*lN#a=B=9pN%tmvYFjFIoKe_(GF z-26x{(KXdfsQL7Uv6UtDuYwV`;8V3w>oT_I<`Ccz3QqK9tYT5ZQzbop{=I=!pMOCb zCU68`n?^DT%^&m>A%+-~#lvF!7`L7a{z<3JqIlk1$<||_J}vW1U9Y&eX<}l8##6i( zZcTT@2`9(Mecptm@{3A_Y(X`w9K0EwtPq~O!16bq{7c0f7#(3wn-^)h zxV&M~iiF!{-6A@>o;$RzQ5A50kxXYj!tcgme=Qjrbje~;5X2xryU;vH|6bE(8z^<7 zQ>BG7_c*JG8~K7Oe68i#0~C$v?-t@~@r3t2inUnLT(c=URpA9kA8uq9PKU(Ps(LVH zqgcqW>Gm?6oV#AldDPKVRcEyQIdTT`Qa1j~vS{<;SwyTdr&3*t?J)y=M7q*CzucZ&B0M=joT zBbj@*SY;o2^_h*>R0e({!QHF0=)0hOj^B^d*m>SnRrwq>MolNSgl^~r8GR#mDWGYEIJA8B<|{{j?-7p zVnV$zancW3&JVDtVpIlI|5djKq0(w$KxEFzEiiL=h5Jw~4Le23@s(mYyXWL9SX6Ot zmb)sZaly_P%BeX_9 zw&{yBef8tFm+%=--m*J|o~+Xg3N+$IH)t)=fqD+|fEk4AAZ&!wcN5=mi~Vvo^i`}> z#_3ahR}Ju)(Px7kev#JGcSwPXJ2id9%Qd2A#Uc@t8~egZ8;iC{e! z%=CGJOD1}j!HW_sgbi_8suYnn4#Ou}%9u)dXd3huFIb!ytlX>Denx@pCS-Nj$`VO&j@(z!kKSP0hE4;YIP#w9ta=3DO$7f*x zc9M4&NK%IrVmZAe=r@skWD`AEWH=g+r|*13Ss$+{c_R!b?>?UaGXlw*8qDmY#xlR= z<0XFbs2t?8i^G~m?b|!Hal^ZjRjt<@a? z%({Gn14b4-a|#uY^=@iiKH+k?~~wTj5K1A&hU z2^9-HTC)7zpoWK|$JXaBL6C z#qSNYtY>65T@Zs&-0cHeu|RX(Pxz6vTITdzJdYippF zC-EB+n4}#lM7`2Ry~SO>FxhKboIAF#Z{1wqxaCb{#yEFhLuX;Rx(Lz%T`Xo1+a2M}7D+@wol2)OJs$TwtRNJ={( zD@#zTUEE}#Fz#&(EoD|SV#bayvr&E0vzmb%H?o~46|FAcx?r4$N z&67W3mdip-T1RIxwSm_&(%U|+WvtGBj*}t69XVd&ebn>KOuL(7Y8cV?THd-(+9>G7*Nt%T zcH;`p={`SOjaf7hNd(=37Lz3-51;58JffzIPgGs_7xIOsB5p2t&@v1mKS$2D$*GQ6 zM(IR*j4{nri7NMK9xlDy-hJW6sW|ZiDRaFiayj%;(%51DN!ZCCCXz+0Vm#};70nOx zJ#yA0P3p^1DED;jGdPbQWo0WATN=&2(QybbVdhd=Vq*liDk`c7iZ?*AKEYC#SY&2g z&Q(Ci)MJ{mEat$ZdSwTjf6h~roanYh2?9j$CF@4hjj_f35kTKuGHvIs9}Re@iKMxS-OI*`0S z6s)fOtz}O$T?PLFVSeOjSO26$@u`e<>k(OSP!&YstH3ANh>)mzmKGNOwOawq-MPXe zy4xbeUAl6tamnx))-`Gi2uV5>9n(73yS)Ukma4*7fI8PaEwa)dWHs6QA6>$}7?(L8 ztN8M}?{Tf!Zu22J5?2@95&rQ|F7=FK-hihT-vDp!5JCcWrVogEnp;CHenAZ)+E+K5 z$Cffk5sNwD_?4+ymgcHR(5xgt20Z8M`2*;MzOM#>yhk{r3x=EyM226wb&!+j`W<%* zSc&|`8!>dn9D@!pYow~(DsY_naSx7(Z4i>cu#hA5=;IuI88}7f%)bRkuY2B;+9Uep zpXcvFWkJ!mQai63BgNXG26$5kyhZ2&*3Q_tk)Ii4M>@p~_~q_cE!|^A;_MHB;7s#9 zKzMzK{lIxotjc};k67^Xsl-gS!^*m*m6kn|sbdun`O?dUkJ{0cmI0-_2y=lTAfn*Y zKg*A-2sJq)CCJgY0LF-VQvl&6HIXZyxo2#!O&6fOhbHXC?%1cMc6y^*dOS{f$=137Ds1m01qs`>iUQ49JijsaQ( zksqV9@&?il$|4Ua%4!O15>Zy&%gBY&wgqB>XA3!EldQ%1CRSM(pp#k~-pkcCg4LAT zXE=puHbgsw)!xtc@P4r~Z}nTF=D2~j(6D%gTBw$(`Fc=OOQ0kiW$_RDd=hcO0t97h zb86S5r=>(@VGy1&#S$Kg_H@7G^;8Ue)X5Y+IWUi`o;mpvoV)`fcVk4FpcT|;EG!;? zHG^zrVVZOm>1KFaHlaogcWj(v!S)O(Aa|Vo?S|P z5|6b{qkH(USa*Z7-y_Uvty_Z1|B{rTS^qmEMLEYUSk03_Fg&!O3BMo{b^*`3SHvl0 zhnLTe^_vVIdcSHe)SQE}r~2dq)VZJ!aSKR?RS<(9lzkYo&dQ?mubnWmgMM37Nudwo z3Vz@R{=m2gENUE3V4NbIzAA$H1z0pagz94-PTJyX{b$yndsdKptmlKQKaaHj@3=ED zc7L?p@%ui|RegVYutK$64q4pe9+5sv34QUpo)u{1ci?)_7gXQd{PL>b0l(LI#rJmN zGuO+%GO`xneFOOr4EU(Wg}_%bhzUf;d@TU+V*2#}!2OLwg~%D;1FAu=Un>OgjPb3S z7l(riiCwgghC=Lm5hWGf5NdGp#01xQ59`HJcLXbUR3&n%P(+W2q$h2Qd z*6+-QXJ*&Kvk9ht0f0*rO_|FMBALen{j7T1l%=Q>gf#kma zQlg#I9+HB+z*5BMxdesMND`_W;q5|FaEURFk|~&{@qY32N$G$2B=&Po{=!)x5b!#n zxLzblkq{yj05#O7(GRuT39(06FJlalyv<#K4m}+vs>9@q-&31@1(QBv82{}Zkns~K ze{eHC_RDX0#^A*JQTwF`a=IkE6Ze@j#-8Q`tTT?k9`^ZhA~3eCZJ-Jr{~7Cx;H4A3 zcZ+Zj{mzFZbVvQ6U~n>$U2ZotGsERZ@}VKrgGh0xM;Jzt29%TX6_&CWzg+YYMozrM z`nutuS)_0dCM8UVaKRj804J4i%z2BA_8A4OJRQ$N(P9Mfn-gF;4#q788C@9XR0O3< zsoS4wIoyt046d+LnSCJOy@B@Uz*#GGd#+Ln1ek5Dv>(ZtD@tgZlPnZZJGBLr^JK+!$$?A_fA3LOrkoDRH&l7 zcMcD$Hsjko3`-{bn)jPL6E9Ds{WskMrivsUu5apD z?grQO@W7i5+%X&E&p|RBaEZ(sGLR@~(y^BI@lDMot^Ll?!`90KT!JXUhYS`ZgX3jnu@Ja^seA*M5R@f`=`ynQV4rc$uT1mvE?@tz)TN<=&H1%Z?5yjxcpO+6y_R z6EPuPKM5uxKpmZfT(WKjRRNHs@ib)F5WAP7QCADvmCSD#hPz$V10wiD&{NXyEwx5S z6NE`3z!IS^$s7m}PCwQutVQ#~w+V z=+~->DI*bR2j0^@dMr9`p>q^Ny~NrAVxrJtX2DUveic5vM%#N*XO|?YAWwNI$Q)_) zvE|L(L1jP@F%gOGtnlXtIv2&1i8q<)Xfz8O3G^Ea~e*HJsQgBxWL(yuLY+jqUK zRE~`-zklrGog(X}$9@ZVUw!8*=l`6mzYLtsg`AvBYz(cxmAhr^j0~(rzXdiOEeu_p zE$sf2(w(BPAvO5DlaN&uQ$4@p-b?fRs}d7&2UQ4Fh?1Hzu*YVjcndqJLw0#q@fR4u zJCJ}>_7-|QbvOfylj+e^_L`5Ep9gqd>XI3-O?Wp z-gt*P29f$Tx(mtS`0d05nHH=gm~Po_^OxxUwV294BDKT>PHVlC5bndncxGR!n(OOm znsNt@Q&N{TLrmsoKFw0&_M9$&+C24`sIXGWgQaz=kY;S{?w`z^Q0JXXBKFLj0w0U6P*+jPKyZHX9F#b0D1$&(- zrm8PJd?+SrVf^JlfTM^qGDK&-p2Kdfg?f>^%>1n8bu&byH(huaocL>l@f%c*QkX2i znl}VZ4R1en4S&Bcqw?$=Zi7ohqB$Jw9x`aM#>pHc0x z0$!q7iFu zZ`tryM70qBI6JWWTF9EjgG@>6SRzsd}3h+4D8d~@CR07P$LJ}MFsYi-*O%XVvD@yT|rJ+Mk zDllJ7$n0V&A!0flbOf)HE6P_afPWZmbhpliqJuw=-h+r;WGk|ntkWN(8tKlYpq5Ow z(@%s>IN8nHRaYb*^d;M(D$zGCv5C|uqmsDjwy4g=Lz>*OhO3z=)VD}C<65;`89Ye} zSCxrv#ILzIpEx1KdLPlM&%Cctf@FqTKvNPXC&`*H9=l=D3r!GLM?UV zOxa(8ZsB`&+76S-_xuj?G#wXBfDY@Z_tMpXJS7^mp z@YX&u0jYw2A+Z+bD#6sgVK5ZgdPSJV3>{K^4~%HV?rn~4D)*2H!67Y>0aOmzup`{D zzDp3c9yEbGCY$U<8biJ_gB*`jluz1ShUd!QUIQJ$*1;MXCMApJ^m*Fiv88RZ zFopLViw}{$Tyhh_{MLGIE2~sZ)t0VvoW%=8qKZ>h=adTe3QM$&$PO2lfqH@brt!9j ziePM8$!CgE9iz6B<6_wyTQj?qYa;eC^{x_0wuwV~W+^fZmFco-o%wsKSnjXFEx02V zF5C2t)T6Gw$Kf^_c;Ei3G~uC8SM-xyycmXyC2hAVi-IfXqhu$$-C=*|X?R0~hu z8`J6TdgflslhrmDZq1f?GXF7*ALeMmOEpRDg(s*H`4>_NAr`2uqF;k;JQ+8>A|_6ZNsNLECC%NNEb1Y1dP zbIEmNpK)#XagtL4R6BC{C5T(+=yA-(Z|Ap}U-AfZM#gwVpus3(gPn}Q$CExObJ5AC z)ff9Yk?wZ}dZ-^)?cbb9Fw#EjqQ8jxF4G3=L?Ra zg_)0QDMV1y^A^>HRI$x?Op@t;oj&H@1xt4SZ9(kifQ zb59B*`M99Td7@aZ3UWvj1rD0sE)d=BsBuW*KwkCds7ay(7*01_+L}b~7)VHI>F_!{ zyxg-&nCO?v#KOUec0{OOKy+sjWA;8rTE|Lv6I9H?CI?H(mUm8VXGwU$49LGpz&{nQp2}dinE1@lZ1iox6{ghN&v^GZv9J${7WaXj)<0S4g_uiJ&JCZ zr8-hsu`U%N;+9N^@&Q0^kVPB3)wY(rr}p7{p0qFHb3NUUHJb672+wRZs`gd1UjKPX z4o6zljKKA+Kkj?H>Ew63o%QjyBk&1!P22;MkD>sM0=z_s-G{mTixJCT9@_|*(p^bz zJ8?ZZ&;pzV+7#6Mn`_U-)k8Pjg?a;|Oe^us^PoPY$Va~yi8|?+&=y$f+lABT<*pZr zP}D{~Pq1Qyni+@|aP;ixO~mbEW9#c0OU#YbDZIaw=_&$K%Ep2f%hO^&P67hApZe`x zv8b`Mz@?M_7-)b!lkQKk)JXXUuT|B8kJlvqRmRpxtQDgvrHMXC1B$M@Y%Me!BSx3P z#2Eawl$HleZhhTS6Txm>lN_+I`>eV$&v9fOg)%zVn3O5mI*lAl>QcHuW6!Kixmq`X zBCZ*Ck6OYtDiK!N47>jxI&O2a9x7M|i^IagRr-fmrmikEQGgw%J7bO|)*$2FW95O4 zeBs>KR)izRG1gRVL;F*sr8A}aRHO0gc$$j&ds8CIO1=Gwq1%_~E)CWNn9pCtBE}+`Jelk4{>S)M)`Ll=!~gnn1yq^EX(+y*ik@3Ou0qU`IgYi3*doM+5&dU!cho$pZ zn%lhKeZkS72P?Cf68<#kll_6OAO26bIbueZx**j6o;I0cS^XiL`y+>{cD}gd%lux} z)3N>MaE24WBZ}s0ApfdM;5J_Ny}rfUyxfkC``Awo2#sgLnGPewK};dORuT?@I6(5~ z?kE)Qh$L&fwJXzK){iYx!l5$Tt|^D~MkGZPA}(o6f7w~O2G6Vvzdo*a;iXzk$B66$ zwF#;wM7A+(;uFG4+UAY(2`*3XXx|V$K8AYu#ECJYSl@S=uZW$ksfC$~qrrbQj4??z-)uz0QL}>k^?fPnJTPw% zGz)~?B4}u0CzOf@l^um}HZzbaIwPmb<)< zi_3@E9lc)Qe2_`*Z^HH;1CXOceL=CHpHS{HySy3T%<^NrWQ}G0i4e1xm_K3(+~oi$ zoHl9wzb?Z4j#90DtURtjtgvi7uw8DzHYmtPb;?%8vb9n@bszT=1qr)V_>R%s!92_` zfnHQPANx z<#hIjIMm#*(v*!OXtF+w8kLu`o?VZ5k7{`vw{Yc^qYclpUGIM_PBN1+c{#Vxv&E*@ zxg=W2W~JuV{IuRYw3>LSI1)a!thID@R=bU+cU@DbR^_SXY`MC7HOsCN z!dO4OKV7(E_Z8T#8MA1H`99?Z!r0)qKW_#|29X3#Jb+5+>qUidbeP1NJ@)(qi2S-X zao|f0_tl(O+$R|Qwd$H{_ig|~I1fbp_$NkI!0E;Y z6JrnU{1Ra6^on{9gUUB0mwzP3S%B#h0fjo>JvV~#+X0P~JV=IG=yHG$O+p5O3NUgG zEQ}z6BTp^Fie)Sg<){Z&I8NwPR(=mO4joTLHkJ>|Tnk23E(Bo`FSbPc05lF2-+)X? z6vV3*m~IBHTy*^E!<0nA(tCOJW2G4DsH7)BxLV8kICn5lu6@U*R`w)o9;Ro$i8=Q^V%uH8n3q=+Yf;SFRZu z!+F&PKcH#8cG?aSK_Tl@K9P#8o+jry@gdexz&d(Q=47<7nw@e@FFfIRNL9^)1i@;A z28+$Z#rjv-wj#heI|<&J_DiJ*s}xd-f!{J8jfqOHE`TiHHZVIA8CjkNQ_u;Ery^^t zl1I75&u^`1_q)crO+JT4rx|z2ToSC>)Or@-D zy3S>jW*sNIZR-EBsfyaJ+Jq4BQE4?SePtD2+jY8*%FsSLZ9MY>+wk?}}}AFAw)vr{ml)8LUG-y9>^t!{~|sgpxYc0Gnkg`&~R z-pilJZjr@y5$>B=VMdZ73svct%##v%wdX~9fz6i3Q-zOKJ9wso+h?VME7}SjL=!NUG{J?M&i!>ma`eoEa@IX`5G>B1(7;%}M*%-# zfhJ(W{y;>MRz!Ic8=S}VaBKqh;~7KdnGEHxcL$kA-6E~=!hrN*zw9N+_=odt<$_H_8dbo;0=42wcAETPCVGUr~v(`Uai zb{=D!Qc!dOEU6v)2eHSZq%5iqK?B(JlCq%T6av$Cb4Rko6onlG&?CqaX7Y_C_cOC3 zYZ;_oI(}=>_07}Oep&Ws7x7-R)cc8zfe!SYxJYP``pi$FDS)4Fvw5HH=FiU6xfVqIM!hJ;Rx8c0cB7~aPtNH(Nmm5Vh{ibAoU#J6 zImRCr?(iyu_4W_6AWo3*vxTPUw@vPwy@E0`(>1Qi=%>5eSIrp^`` zK*Y?fK_6F1W>-7UsB)RPC4>>Ps9)f+^MqM}8AUm@tZ->j%&h1M8s*s!LX5&WxQcAh z8mciQej@RPm?660%>{_D+7er>%zX_{s|$Z+;G7_sfNfBgY(zLB4Ey}J9F>zX#K0f6 z?dVNIeEh?EIShmP6>M+d|0wMM85Sa4diw1hrg|ITJ}JDg@o8y>(rF9mXk5M z2@D|NA)-7>wD&wF;S_$KS=eE84`BGw3g0?6wGxu8ys4rwI?9U=*^VF22t3%mbGeOh z`!O-OpF7#Vceu~F`${bW0nYVU9ecmk31V{tF%iv&5hWofC>I~cqAt@u6|R+|HLMMX zVxuSlMFOK_EQ86#E8&KwxIr8S9tj_goWtLv4f@!&h8;Ov41{J~496vp9vX=(LK#j! zAwi*21RAV-LD>9Cw3bV_9X(X3)Kr0-UaB*7Y>t82EQ%!)(&(XuAYtTsYy-dz+w=$ir)VJpe!_$ z6SGpX^i(af3{o=VlFPC);|J8#(=_8#vdxDe|Cok+ANhYwbE*FO`Su2m1~w+&9<_9~ z-|tTU_ACGN`~CNW5WYYBn^B#SwZ(t4%3aPp z;o)|L6Rk569KGxFLUPx@!6OOa+5OjQLK5w&nAmwxkC5rZ|m&HT8G%GVZxB_@ME z>>{rnXUqyiJrT(8GMj_ap#yN_!9-lO5e8mR3cJiK3NE{_UM&=*vIU`YkiL$1%kf+1 z4=jk@7EEj`u(jy$HnzE33ZVW_J4bj}K;vT?T91YlO(|Y0FU4r+VdbmQ97%(J5 zkK*Bed8+C}FcZ@HIgdCMioV%A<*4pw_n}l*{Cr4}a(lq|injK#O?$tyvyE`S%(1`H z_wwRvk#13ElkZvij2MFGOj`fhy?nC^8`Zyo%yVcUAfEr8x&J#A{|moUBAV_^f$hpaUuyQeY3da^ zS9iRgf87YBwfe}>BO+T&Fl%rfpZh#+AM?Dq-k$Bq`vG6G_b4z%Kbd&v>qFjow*mBl z-OylnqOpLg}or7_VNwRg2za3VBK6FUfFX{|TD z`Wt0Vm2H$vdlRWYQJqDmM?JUbVqL*ZQY|5&sY*?!&%P8qhA~5+Af<{MaGo(dl&C5t zE%t!J0 zh6jqANt4ABdPxSTrVV}fLsRQal*)l&_*rFq(Ez}ClEH6LHv{J#v?+H-BZ2)Wy{K@9 z+ovXHq~DiDvm>O~r$LJo!cOuwL+Oa--6;UFE2q@g3N8Qkw5E>ytz^(&($!O47+i~$ zKM+tkAd-RbmP{s_rh+ugTD;lriL~`Xwkad#;_aM?nQ7L_muEFI}U_4$phjvYgleK~`Fo`;GiC07&Hq1F<%p;9Q;tv5b?*QnR%8DYJH3P>Svmv47Y>*LPZJy8_{9H`g6kQpyZU{oJ`m%&p~D=K#KpfoJ@ zn-3cqmHsdtN!f?~w+(t+I`*7GQA#EQC^lUA9(i6=i1PqSAc|ha91I%X&nXzjYaM{8$s&wEx@aVkQ6M{E2 zfzId#&r(XwUNtPcq4Ngze^+XaJA1EK-%&C9j>^9(secqe{}z>hR5CFNveMsVA)m#S zk)_%SidkY-XmMWlVnQ(mNJ>)ooszQ#vaK;!rPmGKXV7am^_F!Lz>;~{VrIO$;!#30XRhE1QqO_~#+Ux;B_D{Nk=grn z8Y0oR^4RqtcYM)7a%@B(XdbZCOqnX#fD{BQTeLvRHd(irHKq=4*jq34`6@VAQR8WG z^%)@5CXnD_T#f%@-l${>y$tfb>2LPmc{~5A82|16mH)R?&r#KKLs7xpN-D`=&Cm^R zvMA6#Ahr<3X>Q7|-qfTY)}32HkAz$_mibYV!I)u>bmjK`qwBe(>za^0Kt*HnFbSdO z1>+ryKCNxmm^)*$XfiDOF2|{-v3KKB?&!(S_Y=Ht@|ir^hLd978xuI&N{k>?(*f8H z=ClxVJK_%_z1TH0eUwm2J+2To7FK4o+n_na)&#VLn1m;!+CX+~WC+qg1?PA~KdOlC zW)C@pw75_xoe=w7i|r9KGIvQ$+3K?L{7TGHwrQM{dCp=Z*D}3kX7E-@sZnup!BImw z*T#a=+WcTwL78exTgBn|iNE3#EsOorO z*kt)gDzHiPt07fmisA2LWN?AymkdqTgr?=loT7z@d`wnlr6oN}@o|&JX!yPzC*Y8d zu6kWlTzE1)ckyBn+0Y^HMN+GA$wUO_LN6W>mxCo!0?oiQvT`z$jbSEu&{UHRU0E8# z%B^wOc@S!yhMT49Y)ww(Xta^8pmPCe@eI5C*ed96)AX9<>))nKx0(sci8gwob_1}4 z0DIL&vsJ1_s%<@y%U*-eX z5rN&(zef-5G~?@r79oZGW1d!WaTqQn0F6RIOa9tJ=0(kdd{d1{<*tHT#cCvl*i>YY zH+L7jq8xZNcTUBqj(S)ztTU!TM!RQ}In*n&Gn<>(60G7}4%WQL!o>hbJqNDSGwl#H z`4k+twp0cj%PsS+NKaxslAEu9!#U3xT1|_KB6`h=PI0SW`P9GTa7caD1}vKEglV8# zjKZR`pluCW19c2fM&ZG)c3T3Um;ir3y(tSCJ7Agl6|b524dy5El{^EQBG?E61H0XY z`bqg!;zhGhyMFl&(o=JWEJ8n~z)xI}A@C0d2hQGvw7nGv)?POU@(kS1m=%`|+^ika zXl8zjS?xqW$WlO?Ewa;vF~XbybHBor$f<%I&*t$F5fynwZlTGj|IjZtVfGa7l&tK} zW>I<69w(cZLu)QIVG|M2xzW@S+70NinQzk&Y0+3WT*cC)rx~04O-^<{JohU_&HL5XdUKW!uFy|i$FB|EMu0eUyW;gsf`XfIc!Z0V zeK&*hPL}f_cX=@iv>K%S5kL;cl_$v?n(Q9f_cChk8Lq$glT|=e+T*8O4H2n<=NGmn z+2*h+v;kBvF>}&0RDS>)B{1!_*XuE8A$Y=G8w^qGMtfudDBsD5>T5SB;Qo}fSkkiV ze^K^M(UthkwrD!&*tTsu>Dacdj_q`~V%r_twr$(Ct&_dKeeXE?fA&4&yASJWJ*}~- zel=@W)tusynfC_YqH4ll>4Eg`Xjs5F7Tj>tTLz<0N3)X<1px_d2yUY>X~y>>93*$) z5PuNMQLf9Bu?AAGO~a_|J2akO1M*@VYN^VxvP0F$2>;Zb9;d5Yfd8P%oFCCoZE$ z4#N$^J8rxYjUE_6{T%Y>MmWfHgScpuGv59#4u6fpTF%~KB^Ae`t1TD_^Ud#DhL+Dm zbY^VAM#MrAmFj{3-BpVSWph2b_Y6gCnCAombVa|1S@DU)2r9W<> zT5L8BB^er3zxKt1v(y&OYk!^aoQisqU zH(g@_o)D~BufUXcPt!Ydom)e|aW{XiMnes2z&rE?og>7|G+tp7&^;q?Qz5S5^yd$i z8lWr4g5nctBHtigX%0%XzIAB8U|T6&JsC4&^hZBw^*aIcuNO47de?|pGXJ4t}BB`L^d8tD`H`i zqrP8?#J@8T#;{^B!KO6J=@OWKhAerih(phML`(Rg7N1XWf1TN>=Z3Do{l_!d~DND&)O)D>ta20}@Lt77qSnVsA7>)uZAaT9bsB>u&aUQl+7GiY2|dAEg@%Al3i316y;&IhQL^8fw_nwS>f60M_-m+!5)S_6EPM7Y)(Nq^8gL7(3 zOiot`6Wy6%vw~a_H?1hLVzIT^i1;HedHgW9-P#)}Y6vF%C=P70X0Tk^z9Te@kPILI z_(gk!k+0%CG)%!WnBjjw*kAKs_lf#=5HXC00s-}oM-Q1aXYLj)(1d!_a7 z*Gg4Fe6F$*ujVjI|79Z5+Pr`us%zW@ln++2l+0hsngv<{mJ%?OfSo_3HJXOCys{Ug z00*YR-(fv<=&%Q!j%b-_ppA$JsTm^_L4x`$k{VpfLI(FMCap%LFAyq;#ns5bR7V+x zO!o;c5y~DyBPqdVQX)8G^G&jWkBy2|oWTw>)?5u}SAsI$RjT#)lTV&Rf8;>u*qXnb z8F%Xb=7#$m)83z%`E;49)t3fHInhtc#kx4wSLLms!*~Z$V?bTyUGiS&m>1P(952(H zuHdv=;o*{;5#X-uAyon`hP}d#U{uDlV?W?_5UjJvf%11hKwe&(&9_~{W)*y1nR5f_ z!N(R74nNK`y8>B!0Bt_Vr!;nc3W>~RiKtGSBkNlsR#-t^&;$W#)f9tTlZz>n*+Fjz z3zXZ;jf(sTM(oDzJt4FJS*8c&;PLTW(IQDFs_5QPy+7yhi1syPCarvqrHFcf&yTy)^O<1EBx;Ir`5W{TIM>{8w&PB>ro4;YD<5LF^TjTb0!zAP|QijA+1Vg>{Afv^% zmrkc4o6rvBI;Q8rj4*=AZacy*n8B{&G3VJc)so4$XUoie0)vr;qzPZVbb<#Fc=j+8CGBWe$n|3K& z_@%?{l|TzKSlUEO{U{{%Fz_pVDxs7i9H#bnbCw7@4DR=}r_qV!Zo~CvD4ZI*+j3kO zW6_=|S`)(*gM0Z;;}nj`73OigF4p6_NPZQ-Od~e$c_);;4-7sR>+2u$6m$Gf%T{aq zle>e3(*Rt(TPD}03n5)!Ca8Pu!V}m6v0o1;5<1h$*|7z|^(3$Y&;KHKTT}hV056wuF0Xo@mK-52~r=6^SI1NC%c~CC?n>yX6wPTgiWYVz!Sx^atLby9YNn1Rk{g?|pJaxD4|9cUf|V1_I*w zzxK)hRh9%zOl=*$?XUjly5z8?jPMy%vEN)f%T*|WO|bp5NWv@B(K3D6LMl!-6dQg0 zXNE&O>Oyf%K@`ngCvbGPR>HRg5!1IV$_}m@3dWB7x3t&KFyOJn9pxRXCAzFr&%37wXG;z^xaO$ekR=LJG ztIHpY8F5xBP{mtQidqNRoz= z@){+N3(VO5bD+VrmS^YjG@+JO{EOIW)9=F4v_$Ed8rZtHvjpiEp{r^c4F6Ic#ChlC zJX^DtSK+v(YdCW)^EFcs=XP7S>Y!4=xgmv>{S$~@h=xW-G4FF9?I@zYN$e5oF9g$# zb!eVU#J+NjLyX;yb)%SY)xJdvGhsnE*JEkuOVo^k5PyS=o#vq!KD46UTW_%R=Y&0G zFj6bV{`Y6)YoKgqnir2&+sl+i6foAn-**Zd1{_;Zb7Ki=u394C5J{l^H@XN`_6XTKY%X1AgQM6KycJ+= zYO=&t#5oSKB^pYhNdzPgH~aEGW2=ec1O#s-KG z71}LOg@4UEFtp3GY1PBemXpNs6UK-ax*)#$J^pC_me;Z$Je(OqLoh|ZrW*mAMBFn< zHttjwC&fkVfMnQeen8`Rvy^$pNRFVaiEN4Pih*Y3@jo!T0nsClN)pdrr9AYLcZxZ| zJ5Wlj+4q~($hbtuY zVQ7hl>4-+@6g1i`1a)rvtp-;b0>^`Dloy(#{z~ytgv=j4q^Kl}wD>K_Y!l~ zp(_&7sh`vfO(1*MO!B%<6E_bx1)&s+Ae`O)a|X=J9y~XDa@UB`m)`tSG4AUhoM=5& znWoHlA-(z@3n0=l{E)R-p8sB9XkV zZ#D8wietfHL?J5X0%&fGg@MH~(rNS2`GHS4xTo7L$>TPme+Is~!|79=^}QbPF>m%J zFMkGzSndiPO|E~hrhCeo@&Ea{M(ieIgRWMf)E}qeTxT8Q#g-!Lu*x$v8W^M^>?-g= zwMJ$dThI|~M06rG$Sv@C@tWR>_YgaG&!BAbkGggVQa#KdtDB)lMLNVLN|51C@F^y8 zCRvMB^{GO@j=cHfmy}_pCGbP%xb{pNN>? z?7tBz$1^zVaP|uaatYaIN+#xEN4jBzwZ|YI_)p(4CUAz1ZEbDk>J~Y|63SZaak~#0 zoYKruYsWHoOlC1(MhTnsdUOwQfz5p6-D0}4;DO$B;7#M{3lSE^jnTT;ns`>!G%i*F?@pR1JO{QTuD0U+~SlZxcc8~>IB{)@8p`P&+nDxNj`*gh|u?yrv$phpQcW)Us)bi`kT%qLj(fi{dWRZ%Es2!=3mI~UxiW0$-v3vUl?#g{p6eF zMEUAqo5-L0Ar(s{VlR9g=j7+lt!gP!UN2ICMokAZ5(Agd>})#gkA2w|5+<%-CuEP# zqgcM}u@3(QIC^Gx<2dbLj?cFSws_f3e%f4jeR?4M^M3cx1f+Qr6ydQ>n)kz1s##2w zk}UyQc+Z5G-d-1}{WzjkLXgS-2P7auWSJ%pSnD|Uivj5u!xk0 z_^-N9r9o;(rFDt~q1PvE#iJZ_f>J3gcP$)SOqhE~pD2|$=GvpL^d!r z6u=sp-CrMoF7;)}Zd7XO4XihC4ji?>V&(t^?@3Q&t9Mx=qex6C9d%{FE6dvU6%d94 zIE;hJ1J)cCqjv?F``7I*6bc#X)JW2b4f$L^>j{*$R`%5VHFi*+Q$2;nyieduE}qdS{L8y8F08yLs?w}{>8>$3236T-VMh@B zq-nujsb_1aUv_7g#)*rf9h%sFj*^mIcImRV*k~Vmw;%;YH(&ylYpy!&UjUVqqtfG` zox3esju?`unJJA_zKXRJP)rA3nXc$m^{S&-p|v|-0x9LHJm;XIww7C#R$?00l&Yyj z=e}gKUOpsImwW?N)+E(awoF@HyP^EhL+GlNB#k?R<2>95hz!h9sF@U20DHSB3~WMa zk90+858r@-+vWwkawJ)8ougd(i#1m3GLN{iSTylYz$brAsP%=&m$mQQrH$g%3-^VR zE%B`Vi&m8f3T~&myTEK28BDWCVzfWir1I?03;pX))|kY5ClO^+bae z*7E?g=3g7EiisYOrE+lA)2?Ln6q2*HLNpZEWMB|O-JI_oaHZB%CvYB(%=tU= zE*OY%QY58fW#RG5=gm0NR#iMB=EuNF@)%oZJ}nmm=tsJ?eGjia{e{yuU0l3{d^D@)kVDt=1PE)&tf_hHC%0MB znL|CRCPC}SeuVTdf>-QV70`0(EHizc21s^sU>y%hW0t!0&y<7}Wi-wGy>m%(-jsDj zP?mF|>p_K>liZ6ZP(w5(|9Ga%>tLgb$|doDDfkdW>Z z`)>V2XC?NJT26mL^@ zf+IKr27TfM!UbZ@?zRddC7#6ss1sw%CXJ4FWC+t3lHZupzM77m^=9 z&(a?-LxIq}*nvv)y?27lZ{j zifdl9hyJudyP2LpU$-kXctshbJDKS{WfulP5Dk~xU4Le4c#h^(YjJit4#R8_khheS z|8(>2ibaHES4+J|DBM7I#QF5u-*EdN{n=Kt@4Zt?@Tv{JZA{`4 zU#kYOv{#A&gGPwT+$Ud}AXlK3K7hYzo$(fBSFjrP{QQ zeaKg--L&jh$9N}`pu{Bs>?eDFPaWY4|9|foN%}i;3%;@4{dc+iw>m}{3rELqH21G! z`8@;w-zsJ1H(N3%|1B@#ioLOjib)j`EiJqPQVSbPSPVHCj6t5J&(NcWzBrzCiDt{4 zdlPAUKldz%6x5II1H_+jv)(xVL+a;P+-1hv_pM>gMRr%04@k;DTokASSKKhU1Qms| zrWh3a!b(J3n0>-tipg{a?UaKsP7?+|@A+1WPDiQIW1Sf@qDU~M_P65_s}7(gjTn0X zucyEm)o;f8UyshMy&>^SC3I|C6jR*R_GFwGranWZe*I>K+0k}pBuET&M~ z;Odo*ZcT?ZpduHyrf8E%IBFtv;JQ!N_m>!sV6ly$_1D{(&nO~w)G~Y`7sD3#hQk%^ zp}ucDF_$!6DAz*PM8yE(&~;%|=+h(Rn-=1Wykas_-@d&z#=S}rDf`4w(rVlcF&lF! z=1)M3YVz7orwk^BXhslJ8jR);sh^knJW(Qmm(QdSgIAIdlN4Te5KJisifjr?eB{FjAX1a0AB>d?qY4Wx>BZ8&}5K0fA+d{l8 z?^s&l8#j7pR&ijD?0b%;lL9l$P_mi2^*_OL+b}4kuLR$GAf85sOo02?Y#90}CCDiS zZ%rbCw>=H~CBO=C_JVV=xgDe%b4FaEFtuS7Q1##y686r%F6I)s-~2(}PWK|Z8M+Gu zl$y~5@#0Ka%$M<&Cv%L`a8X^@tY&T7<0|(6dNT=EsRe0%kp1Qyq!^43VAKYnr*A5~ zsI%lK1ewqO;0TpLrT9v}!@vJK{QoVa_+N4FYT#h?Y8rS1S&-G+m$FNMP?(8N`MZP zels(*?kK{{^g9DOzkuZXJ2;SrOQsp9T$hwRB1(phw1c7`!Q!by?Q#YsSM#I12RhU{$Q+{xj83axHcftEc$mNJ8_T7A-BQc*k(sZ+~NsO~xAA zxnbb%dam_fZlHvW7fKXrB~F&jS<4FD2FqY?VG?ix*r~MDXCE^WQ|W|WM;gsIA4lQP zJ2hAK@CF*3*VqPr2eeg6GzWFlICi8S>nO>5HvWzyZTE)hlkdC_>pBej*>o0EOHR|) z$?};&I4+_?wvL*g#PJ9)!bc#9BJu1(*RdNEn>#Oxta(VWeM40ola<0aOe2kSS~{^P zDJBd}0L-P#O-CzX*%+$#v;(x%<*SPgAje=F{Zh-@ucd2DA(yC|N_|ocs*|-!H%wEw z@Q!>siv2W;C^^j^59OAX03&}&D*W4EjCvfi(ygcL#~t8XGa#|NPO+*M@Y-)ctFA@I z-p7npT1#5zOLo>7q?aZpCZ=iecn3QYklP;gF0bq@>oyBq94f6C=;Csw3PkZ|5q=(c zfs`aw?II0e(h=|7o&T+hq&m$; zBrE09Twxd9BJ2P+QPN}*OdZ-JZV7%av@OM7v!!NL8R;%WFq*?{9T3{ct@2EKgc8h) zMxoM$SaF#p<`65BwIDfmXG6+OiK0e)`I=!A3E`+K@61f}0e z!2a*FOaDrOe>U`q%K!QN`&=&0C~)CaL3R4VY(NDt{Xz(Xpqru5=r#uQN1L$Je1*dkdqQ*=lofQaN%lO!<5z9ZlHgxt|`THd>2 zsWfU$9=p;yLyJyM^t zS2w9w?Bpto`@H^xJpZDKR1@~^30Il6oFGfk5%g6w*C+VM)+%R@gfIwNprOV5{F^M2 zO?n3DEzpT+EoSV-%OdvZvNF+pDd-ZVZ&d8 zKeIyrrfPN=EcFRCPEDCVflX#3-)Ik_HCkL(ejmY8vzcf-MTA{oHk!R2*36`O68$7J zf}zJC+bbQk--9Xm!u#lgLvx8TXx2J258E5^*IZ(FXMpq$2LUUvhWQPs((z1+2{Op% z?J}9k5^N=z;7ja~zi8a_-exIqWUBJwohe#4QJ`|FF*$C{lM18z^#hX6!5B8KAkLUX ziP=oti-gpV(BsLD{0(3*dw}4JxK23Y7M{BeFPucw!sHpY&l%Ws4pSm`+~V7;bZ%Dx zeI)MK=4vC&5#;2MT7fS?^ch9?2;%<8Jlu-IB&N~gg8t;6S-#C@!NU{`p7M8@2iGc& zg|JPg%@gCoCQ&s6JvDU&`X2S<57f(k8nJ1wvBu{8r?;q3_kpZZ${?|( z+^)UvR33sjSd)aT!UPkA;ylO6{aE3MQa{g%Mcf$1KONcjO@&g5zPHWtzM1rYC{_K> zgQNcs<{&X{OA=cEWw5JGqpr0O>x*Tfak2PE9?FuWtz^DDNI}rwAaT0(bdo-<+SJ6A z&}S%boGMWIS0L}=S>|-#kRX;e^sUsotry(MjE|3_9duvfc|nwF#NHuM-w7ZU!5ei8 z6Mkf>2)WunY2eU@C-Uj-A zG(z0Tz2YoBk>zCz_9-)4a>T46$(~kF+Y{#sA9MWH%5z#zNoz)sdXq7ZR_+`RZ%0(q zC7&GyS_|BGHNFl8Xa%@>iWh%Gr?=J5<(!OEjauj5jyrA-QXBjn0OAhJJ9+v=!LK`` z@g(`^*84Q4jcDL`OA&ZV60djgwG`|bcD*i50O}Q{9_noRg|~?dj%VtKOnyRs$Uzqg z191aWoR^rDX#@iSq0n z?9Sg$WSRPqSeI<}&n1T3!6%Wj@5iw5`*`Btni~G=&;J+4`7g#OQTa>u`{4ZZ(c@s$ zK0y;ySOGD-UTjREKbru{QaS>HjN<2)R%Nn-TZiQ(Twe4p@-saNa3~p{?^V9Nixz@a zykPv~<@lu6-Ng9i$Lrk(xi2Tri3q=RW`BJYOPC;S0Yly%77c727Yj-d1vF!Fuk{Xh z)lMbA69y7*5ufET>P*gXQrxsW+ zz)*MbHZv*eJPEXYE<6g6_M7N%#%mR{#awV3i^PafNv(zyI)&bH?F}2s8_rR(6%!V4SOWlup`TKAb@ee>!9JKPM=&8g#BeYRH9FpFybxBXQI2|g}FGJfJ+ zY-*2hB?o{TVL;Wt_ek;AP5PBqfDR4@Z->_182W z{P@Mc27j6jE*9xG{R$>6_;i=y{qf(c`5w9fa*`rEzX6t!KJ(p1H|>J1pC-2zqWENF zmm=Z5B4u{cY2XYl(PfrInB*~WGWik3@1oRhiMOS|D;acnf-Bs(QCm#wR;@Vf!hOPJ zgjhDCfDj$HcyVLJ=AaTbQ{@vIv14LWWF$=i-BDoC11}V;2V8A`S>_x)vIq44-VB-v z*w-d}$G+Ql?En8j!~ZkCpQ$|cA0|+rrY>tiCeWxkRGPoarxlGU2?7%k#F693RHT24 z-?JsiXlT2PTqZqNb&sSc>$d;O4V@|b6VKSWQb~bUaWn1Cf0+K%`Q&Wc<>mQ>*iEGB zbZ;aYOotBZ{vH3y<0A*L0QVM|#rf*LIsGx(O*-7)r@yyBIzJnBFSKBUSl1e|8lxU* zzFL+YDVVkIuzFWeJ8AbgN&w(4-7zbiaMn{5!JQXu)SELk*CNL+Fro|2v|YO)1l15t zs(0^&EB6DPMyaqvY>=KL>)tEpsn;N5Q#yJj<9}ImL((SqErWN3Q=;tBO~ExTCs9hB z2E$7eN#5wX4<3m^5pdjm#5o>s#eS_Q^P)tm$@SawTqF*1dj_i#)3};JslbLKHXl_N z)Fxzf>FN)EK&Rz&*|6&%Hs-^f{V|+_vL1S;-1K-l$5xiC@}%uDuwHYhmsV?YcOUlk zOYkG5v2+`+UWqpn0aaaqrD3lYdh0*!L`3FAsNKu=Q!vJu?Yc8n|CoYyDo_`r0mPoo z8>XCo$W4>l(==h?2~PoRR*kEe)&IH{1sM41mO#-36`02m#nTX{r*r`Q5rZ2-sE|nA zhnn5T#s#v`52T5|?GNS`%HgS2;R(*|^egNPDzzH_z^W)-Q98~$#YAe)cEZ%vge965AS_am#DK#pjPRr-!^za8>`kksCAUj(Xr*1NW5~e zpypt_eJpD&4_bl_y?G%>^L}=>xAaV>KR6;^aBytqpiHe%!j;&MzI_>Sx7O%F%D*8s zSN}cS^<{iiK)=Ji`FpO#^zY!_|D)qeRNAtgmH)m;qC|mq^j(|hL`7uBz+ULUj37gj zksdbnU+LSVo35riSX_4z{UX=%n&}7s0{WuZYoSfwAP`8aKN9P@%e=~1`~1ASL-z%# zw>DO&ixr}c9%4InGc*_y42bdEk)ZdG7-mTu0bD@_vGAr*NcFoMW;@r?@LUhRI zCUJgHb`O?M3!w)|CPu~ej%fddw20lod?Ufp8Dmt0PbnA0J%KE^2~AIcnKP()025V> zG>noSM3$5Btmc$GZoyP^v1@Poz0FD(6YSTH@aD0}BXva?LphAiSz9f&Y(aDAzBnUh z?d2m``~{z;{}kZJ>a^wYI?ry(V9hIoh;|EFc0*-#*`$T0DRQ1;WsqInG;YPS+I4{g zJGpKk%%Sdc5xBa$Q^_I~(F97eqDO7AN3EN0u)PNBAb+n+ zWBTxQx^;O9o0`=g+Zrt_{lP!sgWZHW?8bLYS$;1a@&7w9rD9|Ge;Gb?sEjFoF9-6v z#!2)t{DMHZ2@0W*fCx;62d#;jouz`R5Y(t{BT=$N4yr^^o$ON8d{PQ=!O zX17^CrdM~7D-;ZrC!||<+FEOxI_WI3CA<35va%4v>gc zEX-@h8esj=a4szW7x{0g$hwoWRQG$yK{@3mqd-jYiVofJE!Wok1* znV7Gm&Ssq#hFuvj1sRyHg(6PFA5U*Q8Rx>-blOs=lb`qa{zFy&n4xY;sd$fE+<3EI z##W$P9M{B3c3Si9gw^jlPU-JqD~Cye;wr=XkV7BSv#6}DrsXWFJ3eUNrc%7{=^sP> zrp)BWKA9<}^R9g!0q7yWlh;gr_TEOD|#BmGq<@IV;ueg+D2}cjpp+dPf&Q(36sFU&K8}hA85U61faW&{ zlB`9HUl-WWCG|<1XANN3JVAkRYvr5U4q6;!G*MTdSUt*Mi=z_y3B1A9j-@aK{lNvx zK%p23>M&=KTCgR!Ee8c?DAO2_R?B zkaqr6^BSP!8dHXxj%N1l+V$_%vzHjqvu7p@%Nl6;>y*S}M!B=pz=aqUV#`;h%M0rU zHfcog>kv3UZAEB*g7Er@t6CF8kHDmKTjO@rejA^ULqn!`LwrEwOVmHx^;g|5PHm#B zZ+jjWgjJ!043F+&#_;D*mz%Q60=L9Ove|$gU&~As5^uz@2-BfQ!bW)Khn}G+Wyjw- z19qI#oB(RSNydn0t~;tAmK!P-d{b-@@E5|cdgOS#!>%#Rj6ynkMvaW@37E>@hJP^8 z2zk8VXx|>#R^JCcWdBCy{0nPmYFOxN55#^-rlqobe0#L6)bi?E?SPymF*a5oDDeSd zO0gx?#KMoOd&G(2O@*W)HgX6y_aa6iMCl^~`{@UR`nMQE`>n_{_aY5nA}vqU8mt8H z`oa=g0SyiLd~BxAj2~l$zRSDHxvDs;I4>+M$W`HbJ|g&P+$!U7-PHX4RAcR0szJ*( ze-417=bO2q{492SWrqDK+L3#ChUHtz*@MP)e^%@>_&#Yk^1|tv@j4%3T)diEX zATx4K*hcO`sY$jk#jN5WD<=C3nvuVsRh||qDHnc~;Kf59zr0;c7VkVSUPD%NnnJC_ zl3F^#f_rDu8l}l8qcAz0FFa)EAt32IUy_JLIhU_J^l~FRH&6-ivSpG2PRqzDdMWft>Zc(c)#tb%wgmWN%>IOPm zZi-noqS!^Ftb81pRcQi`X#UhWK70hy4tGW1mz|+vI8c*h@ zfFGJtW3r>qV>1Z0r|L>7I3un^gcep$AAWfZHRvB|E*kktY$qQP_$YG60C@X~tTQjB3%@`uz!qxtxF+LE!+=nrS^07hn` zEgAp!h|r03h7B!$#OZW#ACD+M;-5J!W+{h|6I;5cNnE(Y863%1(oH}_FTW})8zYb$7czP zg~Szk1+_NTm6SJ0MS_|oSz%e(S~P-&SFp;!k?uFayytV$8HPwuyELSXOs^27XvK-D zOx-Dl!P|28DK6iX>p#Yb%3`A&CG0X2S43FjN%IB}q(!hC$fG}yl1y9W&W&I@KTg6@ zK^kpH8=yFuP+vI^+59|3%Zqnb5lTDAykf z9S#X`3N(X^SpdMyWQGOQRjhiwlj!0W-yD<3aEj^&X%=?`6lCy~?`&WSWt z?U~EKFcCG_RJ(Qp7j=$I%H8t)Z@6VjA#>1f@EYiS8MRHZphp zMA_5`znM=pzUpBPO)pXGYpQ6gkine{6u_o!P@Q+NKJ}k!_X7u|qfpAyIJb$_#3@wJ z<1SE2Edkfk9C!0t%}8Yio09^F`YGzpaJHGk*-ffsn85@)%4@`;Fv^8q(-Wk7r=Q8p zT&hD`5(f?M{gfzGbbwh8(}G#|#fDuk7v1W)5H9wkorE0ZZjL0Q1=NRGY>zwgfm81DdoaVwNH;or{{eSyybt)m<=zXoA^RALYG-2t zouH|L*BLvmm9cdMmn+KGopyR@4*=&0&4g|FLoreZOhRmh=)R0bg~ zT2(8V_q7~42-zvb)+y959OAv!V$u(O3)%Es0M@CRFmG{5sovIq4%8Ahjk#*5w{+)+ zMWQoJI_r$HxL5km1#6(e@{lK3Udc~n0@g`g$s?VrnQJ$!oPnb?IHh-1qA`Rz$)Ai< z6w$-MJW-gKNvOhL+XMbE7&mFt`x1KY>k4(!KbbpZ`>`K@1J<(#vVbjx@Z@(6Q}MF# zMnbr-f55(cTa^q4+#)=s+ThMaV~E`B8V=|W_fZWDwiso8tNMTNse)RNBGi=gVwgg% zbOg8>mbRN%7^Um-7oj4=6`$|(K7!+t^90a{$18Z>}<#!bm%ZEFQ{X(yBZMc>lCz0f1I2w9Sq zuGh<9<=AO&g6BZte6hn>Qmvv;Rt)*cJfTr2=~EnGD8P$v3R|&1RCl&7)b+`=QGapi zPbLg_pxm`+HZurtFZ;wZ=`Vk*do~$wB zxoW&=j0OTbQ=Q%S8XJ%~qoa3Ea|au5o}_(P;=!y-AjFrERh%8la!z6Fn@lR?^E~H12D?8#ht=1F;7@o4$Q8GDj;sSC%Jfn01xgL&%F2 zwG1|5ikb^qHv&9hT8w83+yv&BQXOQyMVJSBL(Ky~p)gU3#%|blG?IR9rP^zUbs7rOA0X52Ao=GRt@C&zlyjNLv-} z9?*x{y(`509qhCV*B47f2hLrGl^<@SuRGR!KwHei?!CM10Tq*YDIoBNyRuO*>3FU? zHjipIE#B~y3FSfOsMfj~F9PNr*H?0oHyYB^G(YyNh{SxcE(Y-`x5jFMKb~HO*m+R% zrq|ic4fzJ#USpTm;X7K+E%xsT_3VHKe?*uc4-FsILUH;kL>_okY(w`VU*8+l>o>Jm ziU#?2^`>arnsl#)*R&nf_%>A+qwl%o{l(u)M?DK1^mf260_oteV3#E_>6Y4!_hhVD zM8AI6MM2V*^_M^sQ0dmHu11fy^kOqXqzpr?K$`}BKWG`=Es(9&S@K@)ZjA{lj3ea7_MBP zk(|hBFRjHVMN!sNUkrB;(cTP)T97M$0Dtc&UXSec<+q?y>5=)}S~{Z@ua;1xt@=T5 zI7{`Z=z_X*no8s>mY;>BvEXK%b`a6(DTS6t&b!vf_z#HM{Uoy_5fiB(zpkF{})ruka$iX*~pq1ZxD?q68dIo zIZSVls9kFGsTwvr4{T_LidcWtt$u{kJlW7moRaH6+A5hW&;;2O#$oKyEN8kx`LmG)Wfq4ykh+q{I3|RfVpkR&QH_x;t41Uw z`P+tft^E2B$domKT@|nNW`EHwyj>&}K;eDpe z1bNOh=fvIfk`&B61+S8ND<(KC%>y&?>opCnY*r5M+!UrWKxv0_QvTlJc>X#AaI^xo zaRXL}t5Ej_Z$y*|w*$6D+A?Lw-CO-$itm^{2Ct82-<0IW)0KMNvJHgBrdsIR0v~=H z?n6^}l{D``Me90`^o|q!olsF?UX3YSq^6Vu>Ijm>>PaZI8G@<^NGw{Cx&%|PwYrfw zR!gX_%AR=L3BFsf8LxI|K^J}deh0ZdV?$3r--FEX`#INxsOG6_=!v)DI>0q|BxT)z z-G6kzA01M?rba+G_mwNMQD1mbVbNTWmBi*{s_v_Ft9m2Avg!^78(QFu&n6mbRJ2bA zv!b;%yo{g*9l2)>tsZJOOp}U~8VUH`}$ z8p_}t*XIOehezolNa-a2x0BS})Y9}&*TPgua{Ewn-=wVrmJUeU39EKx+%w%=ixQWK zDLpwaNJs65#6o7Ln7~~X+p_o2BR1g~VCfxLzxA{HlWAI6^H;`juI=&r1jQrUv_q0Z z1Ja-tjdktrrP>GOC*#p?*xfQU5MqjMsBe!9lh(u8)w$e@Z|>aUHI5o;MGw*|Myiz3 z-f0;pHg~Q#%*Kx8MxH%AluVXjG2C$)WL-K63@Q`#y9_k_+}eR(x4~dp7oV-ek0H>I zgy8p#i4GN{>#v=pFYUQT(g&b$OeTy-X_#FDgNF8XyfGY6R!>inYn8IR2RDa&O!(6< znXs{W!bkP|s_YI*Yx%4stI`=ZO45IK6rBs`g7sP40ic}GZ58s?Mc$&i`kq_tfci>N zIHrC0H+Qpam1bNa=(`SRKjixBTtm&e`j9porEci!zdlg1RI0Jw#b(_Tb@RQK1Zxr_ z%7SUeH6=TrXt3J@js`4iDD0=IoHhK~I7^W8^Rcp~Yaf>2wVe|Hh1bUpX9ATD#moByY57-f2Ef1TP^lBi&p5_s7WGG9|0T}dlfxOx zXvScJO1Cnq`c`~{Dp;{;l<-KkCDE+pmexJkd}zCgE{eF=)K``-qC~IT6GcRog_)!X z?fK^F8UDz$(zFUrwuR$qro5>qqn>+Z%<5>;_*3pZ8QM|yv9CAtrAx;($>4l^_$_-L z*&?(77!-=zvnCVW&kUcZMb6;2!83si518Y%R*A3JZ8Is|kUCMu`!vxDgaWjs7^0j( ziTaS4HhQ)ldR=r)_7vYFUr%THE}cPF{0H45FJ5MQW^+W>P+eEX2kLp3zzFe*-pFVA zdDZRybv?H|>`9f$AKVjFWJ=wegO7hOOIYCtd?Vj{EYLT*^gl35|HQ`R=ti+ADm{jyQE7K@kdjuqJhWVSks>b^ zxha88-h3s;%3_5b1TqFCPTxVjvuB5U>v=HyZ$?JSk+&I%)M7KE*wOg<)1-Iy)8-K! z^XpIt|0ibmk9RtMmlUd7#Ap3Q!q9N4atQy)TmrhrFhfx1DAN`^vq@Q_SRl|V z#lU<~n67$mT)NvHh`%als+G-)x1`Y%4Bp*6Un5Ri9h=_Db zA-AdP!f>f0m@~>7X#uBM?diI@)Egjuz@jXKvm zJo+==juc9_<;CqeRaU9_Mz@;3e=E4=6TK+c`|uu#pIqhSyNm`G(X)&)B`8q0RBv#> z`gGlw(Q=1Xmf55VHj%C#^1lpc>LY8kfA@|rlC1EA<1#`iuyNO z(=;irt{_&K=i4)^x%;U(Xv<)+o=dczC5H3W~+e|f~{*ucxj@{Yi-cw^MqYr3fN zF5D+~!wd$#al?UfMnz(@K#wn`_5na@rRr8XqN@&M&FGEC@`+OEv}sI1hw>Up0qAWf zL#e4~&oM;TVfjRE+10B_gFlLEP9?Q-dARr3xi6nQqnw>k-S;~b z;!0s2VS4}W8b&pGuK=7im+t(`nz@FnT#VD|!)eQNp-W6)@>aA+j~K*H{$G`y2|QHY z|Hmy+CR@#jWY4~)lr1qBJB_RfHJFfP<}pK5(#ZZGSqcpyS&}01LnTWk5fzmXMGHkJ zTP6L^B+uj;lmB_W<~4=${+v0>z31M!-_O@o-O9GyW)j_mjx}!0@br_LE-7SIuPP84 z;5=O(U*g_um0tyG|61N@d9lEuOeiRd+#NY^{nd5;-CVlw&Ap7J?qwM^?E29wvS}2d zbzar4Fz&RSR(-|s!Z6+za&Z zY#D<5q_JUktIzvL0)yq_kLWG6DO{ri=?c!y!f(Dk%G{8)k`Gym%j#!OgXVDD3;$&v@qy#ISJfp=Vm>pls@9-mapVQChAHHd-x+OGx)(*Yr zC1qDUTZ6mM(b_hi!TuFF2k#8uI2;kD70AQ&di$L*4P*Y-@p`jdm%_c3f)XhYD^6M8&#Y$ZpzQMcR|6nsH>b=*R_Von!$BTRj7yGCXokoAQ z&ANvx0-Epw`QIEPgI(^cS2f(Y85yV@ygI{ewyv5Frng)e}KCZF7JbR(&W618_dcEh(#+^zZFY;o<815<5sOHQdeax9_!PyM&;{P zkBa5xymca0#)c#tke@3KNEM8a_mT&1gm;p&&JlMGH(cL(b)BckgMQ^9&vRwj!~3@l zY?L5}=Jzr080OGKb|y`ee(+`flQg|!lo6>=H)X4`$Gz~hLmu2a%kYW_Uu8x09Pa0J zKZ`E$BKJ=2GPj_3l*TEcZ*uYRr<*J^#5pILTT;k_cgto1ZL-%slyc16J~OH-(RgDA z%;EjEnoUkZ&acS{Q8`{i6T5^nywgqQI5bDIymoa7CSZG|WWVk>GM9)zy*bNih|QIm z%0+(Nnc*a_xo;$=!HQYaapLms>J1ToyjtFByY`C2H1wT#178#4+|{H0BBqtCdd$L% z_3Hc60j@{t9~MjM@LBalR&6@>B;9?r<7J~F+WXyYu*y3?px*=8MAK@EA+jRX8{CG?GI-< z54?Dc9CAh>QTAvyOEm0^+x;r2BWX|{3$Y7)L5l*qVE*y0`7J>l2wCmW zL1?|a`pJ-l{fb_N;R(Z9UMiSj6pQjOvQ^%DvhIJF!+Th7jO2~1f1N+(-TyCFYQZYw z4)>7caf^Ki_KJ^Zx2JUb z&$3zJy!*+rCV4%jqwyuNY3j1ZEiltS0xTzd+=itTb;IPYpaf?8Y+RSdVdpacB(bVQ zC(JupLfFp8y43%PMj2}T|VS@%LVp>hv4Y!RPMF?pp8U_$xCJ)S zQx!69>bphNTIb9yn*_yfj{N%bY)t{L1cs8<8|!f$;UQ*}IN=2<6lA;x^(`8t?;+ST zh)z4qeYYgZkIy{$4x28O-pugO&gauRh3;lti9)9Pvw+^)0!h~%m&8Q!AKX%urEMnl z?yEz?g#ODn$UM`+Q#$Q!6|zsq_`dLO5YK-6bJM6ya>}H+vnW^h?o$z;V&wvuM$dR& zeEq;uUUh$XR`TWeC$$c&Jjau2it3#%J-y}Qm>nW*s?En?R&6w@sDXMEr#8~$=b(gk zwDC3)NtAP;M2BW_lL^5ShpK$D%@|BnD{=!Tq)o(5@z3i7Z){} zGr}Exom_qDO{kAVkZ*MbLNHE666Kina#D{&>Jy%~w7yX$oj;cYCd^p9zy z8*+wgSEcj$4{WxKmCF(5o7U4jqwEvO&dm1H#7z}%VXAbW&W24v-tS6N3}qrm1OnE)fUkoE8yMMn9S$?IswS88tQWm4#Oid#ckgr6 zRtHm!mfNl-`d>O*1~d7%;~n+{Rph6BBy^95zqI{K((E!iFQ+h*C3EsbxNo_aRm5gj zKYug($r*Q#W9`p%Bf{bi6;IY0v`pB^^qu)gbg9QHQ7 zWBj(a1YSu)~2RK8Pi#C>{DMlrqFb9e_RehEHyI{n?e3vL_}L>kYJC z_ly$$)zFi*SFyNrnOt(B*7E$??s67EO%DgoZL2XNk8iVx~X_)o++4oaK1M|ou73vA0K^503j@uuVmLcHH4ya-kOIDfM%5%(E z+Xpt~#7y2!KB&)PoyCA+$~DXqxPxxALy!g-O?<9+9KTk4Pgq4AIdUkl`1<1#j^cJg zgU3`0hkHj_jxV>`Y~%LAZl^3o0}`Sm@iw7kwff{M%VwtN)|~!p{AsfA6vB5UolF~d zHWS%*uBDt<9y!9v2Xe|au&1j&iR1HXCdyCjxSgG*L{wmTD4(NQ=mFjpa~xooc6kju z`~+d{j7$h-;HAB04H!Zscu^hZffL#9!p$)9>sRI|Yovm)g@F>ZnosF2EgkU3ln0bR zTA}|+E(tt)!SG)-bEJi_0m{l+(cAz^pi}`9=~n?y&;2eG;d9{M6nj>BHGn(KA2n|O zt}$=FPq!j`p&kQ8>cirSzkU0c08%8{^Qyqi-w2LoO8)^E7;;I1;HQ6B$u0nNaX2CY zSmfi)F`m94zL8>#zu;8|{aBui@RzRKBlP1&mfFxEC@%cjl?NBs`cr^nm){>;$g?rhKr$AO&6qV_Wbn^}5tfFBry^e1`%du2~o zs$~dN;S_#%iwwA_QvmMjh%Qo?0?rR~6liyN5Xmej8(*V9ym*T`xAhHih-v$7U}8=dfXi2i*aAB!xM(Xekg*ix@r|ymDw*{*s0?dlVys2e)z62u1 z+k3esbJE=-P5S$&KdFp+2H7_2e=}OKDrf( z9-207?6$@f4m4B+9E*e((Y89!q?zH|mz_vM>kp*HGXldO0Hg#!EtFhRuOm$u8e~a9 z5(roy7m$Kh+zjW6@zw{&20u?1f2uP&boD}$#Zy)4o&T;vyBoqFiF2t;*g=|1=)PxB z8eM3Mp=l_obbc?I^xyLz?4Y1YDWPa+nm;O<$Cn;@ane616`J9OO2r=rZr{I_Kizyc zP#^^WCdIEp*()rRT+*YZK>V@^Zs=ht32x>Kwe zab)@ZEffz;VM4{XA6e421^h~`ji5r%)B{wZu#hD}f3$y@L0JV9f3g{-RK!A?vBUA}${YF(vO4)@`6f1 z-A|}e#LN{)(eXloDnX4Vs7eH|<@{r#LodP@Nz--$Dg_Par%DCpu2>2jUnqy~|J?eZ zBG4FVsz_A+ibdwv>mLp>P!(t}E>$JGaK$R~;fb{O3($y1ssQQo|5M;^JqC?7qe|hg zu0ZOqeFcp?qVn&Qu7FQJ4hcFi&|nR!*j)MF#b}QO^lN%5)4p*D^H+B){n8%VPUzi! zDihoGcP71a6!ab`l^hK&*dYrVYzJ0)#}xVrp!e;lI!+x+bfCN0KXwUAPU9@#l7@0& QuEJmfE|#`Dqx|px0L@K;Y5)KL diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index 538afc79..5e82d67b 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,7 @@ distributionBase=GRADLE_USER_HOME distributionPath=permwrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-7.6.1-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-8.5-bin.zip +networkTimeout=10000 +validateDistributionUrl=true zipStoreBase=GRADLE_USER_HOME zipStorePath=permwrapper/dists diff --git a/gradlew b/gradlew index c4ea8564..1aa94a42 100755 --- a/gradlew +++ b/gradlew @@ -1,4 +1,5 @@ #!/bin/sh + # # Copyright © 2015-2021 the original authors. # @@ -54,7 +55,7 @@ # Darwin, MinGW, and NonStop. # # (3) This script is generated from the Groovy template -# https://github.com/gradle/gradle/blob/master/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# https://github.com/gradle/gradle/blob/HEAD/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt # within the Gradle project. # # You can find Gradle at https://github.com/gradle/gradle/. @@ -68,37 +69,35 @@ app_path=$0 # Need this for daisy-chained symlinks. while - APP_HOME=${app_path%"${app_path##*/}"} # leaves a trailing /; empty if no leading path - [ -h "$app_path" ] + APP_HOME=${app_path%"${app_path##*/}"} # leaves a trailing /; empty if no leading path + [ -h "$app_path" ] do - ls=$( ls -ld "$app_path" ) - link=${ls#*' -> '} - case $link in #( - /*) app_path=$link ;; #( - *) app_path=$APP_HOME$link ;; - esac + ls=$( ls -ld "$app_path" ) + link=${ls#*' -> '} + case $link in #( + /*) app_path=$link ;; #( + *) app_path=$APP_HOME$link ;; + esac done -APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit - -APP_NAME="Gradle" +# This is normally unused +# shellcheck disable=SC2034 APP_BASE_NAME=${0##*/} - -# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. -DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' +# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) +APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit # Use the maximum available, or set MAX_FD != -1 to use that value. MAX_FD=maximum warn () { - echo "$*" + echo "$*" } >&2 die () { - echo - echo "$*" - echo - exit 1 + echo + echo "$*" + echo + exit 1 } >&2 # OS specific support (must be 'true' or 'false'). @@ -107,10 +106,10 @@ msys=false darwin=false nonstop=false case "$( uname )" in #( -CYGWIN* ) cygwin=true ;; #( -Darwin* ) darwin=true ;; #( -MSYS* | MINGW* ) msys=true ;; #( -NONSTOP* ) nonstop=true ;; + CYGWIN* ) cygwin=true ;; #( + Darwin* ) darwin=true ;; #( + MSYS* | MINGW* ) msys=true ;; #( + NONSTOP* ) nonstop=true ;; esac CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar @@ -118,39 +117,46 @@ CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar # Determine the Java command to use to start the JVM. if [ -n "$JAVA_HOME" ] ; then - if [ -x "$JAVA_HOME/jre/sh/java" ] ; then - # IBM's JDK on AIX uses strange locations for the executables - JAVACMD=$JAVA_HOME/jre/sh/java - else - JAVACMD=$JAVA_HOME/bin/java - fi - if [ ! -x "$JAVACMD" ] ; then - die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME + if [ -x "$JAVA_HOME/jre/sh/java" ] ; then + # IBM's JDK on AIX uses strange locations for the executables + JAVACMD=$JAVA_HOME/jre/sh/java + else + JAVACMD=$JAVA_HOME/bin/java + fi + if [ ! -x "$JAVACMD" ] ; then + die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME Please set the JAVA_HOME variable in your environment to match the location of your Java installation." - fi + fi else - JAVACMD=java - which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + JAVACMD=java + if ! command -v java >/dev/null 2>&1 + then + die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. Please set the JAVA_HOME variable in your environment to match the location of your Java installation." + fi fi # Increase the maximum file descriptors if we can. if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then - case $MAX_FD in #( - max*) - MAX_FD=$( ulimit -H -n ) || - warn "Could not query maximum file descriptor limit" - esac - case $MAX_FD in #( - '' | soft) :;; #( - *) - ulimit -n "$MAX_FD" || - warn "Could not set maximum file descriptor limit to $MAX_FD" - esac + case $MAX_FD in #( + max*) + # In POSIX sh, ulimit -H is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 + MAX_FD=$( ulimit -H -n ) || + warn "Could not query maximum file descriptor limit" + esac + case $MAX_FD in #( + '' | soft) :;; #( + *) + # In POSIX sh, ulimit -n is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 + ulimit -n "$MAX_FD" || + warn "Could not set maximum file descriptor limit to $MAX_FD" + esac fi # Collect all arguments for the java command, stacking in reverse order: @@ -163,46 +169,56 @@ fi # For Cygwin or MSYS, switch paths to Windows format before running java if "$cygwin" || "$msys" ; then - APP_HOME=$( cygpath --path --mixed "$APP_HOME" ) - CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" ) - - JAVACMD=$( cygpath --unix "$JAVACMD" ) - - # Now convert the arguments - kludge to limit ourselves to /bin/sh - for arg do - if - case $arg in #( - -*) false ;; # don't mess with options #( - /?*) t=${arg#/} t=/${t%%/*} # looks like a POSIX filepath - [ -e "$t" ] ;; #( - *) false ;; - esac - then - arg=$( cygpath --path --ignore --mixed "$arg" ) - fi - # Roll the args list around exactly as many times as the number of - # args, so each arg winds up back in the position where it started, but - # possibly modified. - # - # NB: a `for` loop captures its iteration list before it begins, so - # changing the positional parameters here affects neither the number of - # iterations, nor the values presented in `arg`. - shift # remove old arg - set -- "$@" "$arg" # push replacement arg - done + APP_HOME=$( cygpath --path --mixed "$APP_HOME" ) + CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" ) + + JAVACMD=$( cygpath --unix "$JAVACMD" ) + + # Now convert the arguments - kludge to limit ourselves to /bin/sh + for arg do + if + case $arg in #( + -*) false ;; # don't mess with options #( + /?*) t=${arg#/} t=/${t%%/*} # looks like a POSIX filepath + [ -e "$t" ] ;; #( + *) false ;; + esac + then + arg=$( cygpath --path --ignore --mixed "$arg" ) + fi + # Roll the args list around exactly as many times as the number of + # args, so each arg winds up back in the position where it started, but + # possibly modified. + # + # NB: a `for` loop captures its iteration list before it begins, so + # changing the positional parameters here affects neither the number of + # iterations, nor the values presented in `arg`. + shift # remove old arg + set -- "$@" "$arg" # push replacement arg + done fi -# Collect all arguments for the java command; -# * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of -# shell script including quotes and variable substitutions, so put them in -# double quotes to make sure that they get re-expanded; and -# * put everything else in single quotes, so that it's not re-expanded. + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' + +# Collect all arguments for the java command: +# * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments, +# and any embedded shellness will be escaped. +# * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be +# treated as '${Hostname}' itself on the command line. set -- \ - "-Dorg.gradle.appname=$APP_BASE_NAME" \ - -classpath "$CLASSPATH" \ - org.gradle.wrapper.GradleWrapperMain \ - "$@" + "-Dorg.gradle.appname=$APP_BASE_NAME" \ + -classpath "$CLASSPATH" \ + org.gradle.wrapper.GradleWrapperMain \ + "$@" + +# Stop when "xargs" is not available. +if ! command -v xargs >/dev/null 2>&1 +then + die "xargs is not available" +fi # Use "xargs" to parse quoted args. # @@ -224,10 +240,10 @@ set -- \ # eval "set -- $( - printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" | - xargs -n1 | - sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' | - tr '\n' ' ' - )" '"$@"' + printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" | + xargs -n1 | + sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' | + tr '\n' ' ' + )" '"$@"' exec "$JAVACMD" "$@" diff --git a/gradlew.bat b/gradlew.bat index 107acd32..93e3f59f 100644 --- a/gradlew.bat +++ b/gradlew.bat @@ -14,7 +14,7 @@ @rem limitations under the License. @rem -@if "%DEBUG%" == "" @echo off +@if "%DEBUG%"=="" @echo off @rem ########################################################################## @rem @rem Gradle startup script for Windows @@ -25,7 +25,8 @@ if "%OS%"=="Windows_NT" setlocal set DIRNAME=%~dp0 -if "%DIRNAME%" == "" set DIRNAME=. +if "%DIRNAME%"=="" set DIRNAME=. +@rem This is normally unused set APP_BASE_NAME=%~n0 set APP_HOME=%DIRNAME% @@ -40,7 +41,7 @@ if defined JAVA_HOME goto findJavaFromJavaHome set JAVA_EXE=java.exe %JAVA_EXE% -version >NUL 2>&1 -if "%ERRORLEVEL%" == "0" goto execute +if %ERRORLEVEL% equ 0 goto execute echo. echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. @@ -75,13 +76,15 @@ set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar :end @rem End local scope for the variables with windows NT shell -if "%ERRORLEVEL%"=="0" goto mainEnd +if %ERRORLEVEL% equ 0 goto mainEnd :fail rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of rem the _cmd.exe /c_ return code! -if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1 -exit /b 1 +set EXIT_CODE=%ERRORLEVEL% +if %EXIT_CODE% equ 0 set EXIT_CODE=1 +if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE% +exit /b %EXIT_CODE% :mainEnd if "%OS%"=="Windows_NT" endlocal diff --git a/gradlew.ps1 b/gradlew.ps1 deleted file mode 100644 index 2d4b187b..00000000 --- a/gradlew.ps1 +++ /dev/null @@ -1,76 +0,0 @@ -# ########################################################################## -# -# Gradle startup script for Windows Powershell. -# -# Ported from gradlew.bat for use on Windows where .bat files are blocked. -# -# Can be use from Powershell Terminal in Visual Studio Code. -# Example usage to build : -# .\gradlew.ps1 build -# -# TODO : Does it help to install powershell extensions plugin in VSCode?? -# -# Original gradlew.bat file is licensed under the Apache license. -################################################################################ -# -# Copyright 2015 the original author or authors. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# -# ########################################################################## - -$APP_HOME = Split-Path $MyInvocation.MyCommand.Path -$APP_NAME = "gradlew" - -# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. -$DEFAULT_JVM_OPTS = "-Xms64m" + " " + "-Xmx64m" - -# Find java.exe -if (! $env:JAVA_HOME) { -$JAVA_EXE = "java.exe" -$p = '$JAVA_EXE -version >NUL 2>&1' -if($p.ExitCode) { - Write-Output "`n" - Write-Output "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH." - Write-Output "`n" - Write-Output "Please set the JAVA_HOME variable in your environment to match the" - Write-Output "location of your Java installation." - exit 1 -} -} else { -$JAVA_EXE="$env:JAVA_HOME/bin/java.exe" -if (-not (Test-Path -Path $JAVA_EXE -PathType Leaf)) { - Write-Output "`n" - Write-Output "ERROR: JAVA_HOME is set to an invalid directory: $env:JAVA_HOME" - Write-Output "`n" - Write-Output "Please set the JAVA_HOME variable in your environment to match the" - Write-Output "location of your Java installation." - exit 1 -} -} - -# Include command line arguments -$CMD_LINE_ARGS = "" -foreach ($element in $args) { -$CMD_LINE_ARGS += "$element " -} - -# Setup the CLASSPATH -$CLASSPATH = "$APP_HOME\gradle\wrapper\gradle-wrapper.jar" - -# Setup parameters -$params = "$DEFAULT_JVM_OPTS"+" "+"$env:JAVA_OPTS"+" "+"$env:GRADLE_OPTS"+" "+"-Dorg.gradle.appname=$APP_NAME"+" "+"-classpath $CLASSPATH"+" "+"org.gradle.wrapper.GradleWrapperMain"+" "+"$CMD_LINE_ARGS" -$cmd = "$JAVA_EXE" -# Execute Gradle -$g = Start-Process -Wait -PassThru -NoNewWindow -FilePath "$cmd" -ArgumentList $params -exit $g.ExitCode diff --git a/settings.gradle b/settings.gradle index 8f24603d..d94f73c6 100644 --- a/settings.gradle +++ b/settings.gradle @@ -1,31 +1,30 @@ import org.gradle.internal.os.OperatingSystem pluginManagement { - repositories { - mavenLocal() - gradlePluginPortal() - String frcYear = '2024' - File frcHome - if (OperatingSystem.current().isWindows()) { - String publicFolder = System.getenv('PUBLIC') - if (publicFolder == null) { - publicFolder = "C:\\Users\\Public" - } - def homeRoot = new File(publicFolder, "wpilib") - frcHome = new File(homeRoot, frcYear) - } else { - def userFolder = System.getProperty("user.home") - def homeRoot = new File(userFolder, "wpilib") - frcHome = new File(homeRoot, frcYear) - } - def frcHomeMaven = new File(frcHome, 'maven') - maven { - name 'frcHome' - url frcHomeMaven - } - } - - plugins { - id 'net.ltgt.errorprone' version '2.0.2' - } + repositories { + mavenLocal() + gradlePluginPortal() + String frcYear = '2024' + File frcHome + if (OperatingSystem.current().isWindows()) { + String publicFolder = System.getenv('PUBLIC') + if (publicFolder == null) { + publicFolder = "C:\\Users\\Public" + } + def homeRoot = new File(publicFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } else { + def userFolder = System.getProperty("user.home") + def homeRoot = new File(userFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } + def frcHomeMaven = new File(frcHome, 'maven') + maven { + name 'frcHome' + url frcHomeMaven + } + } } + +Properties props = System.getProperties(); +props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true"); diff --git a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java index 7ffabff4..1b24c8c3 100644 --- a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java @@ -75,6 +75,7 @@ public DrivebaseSubsystem() { MAX_AUTO_SPEED, DRIVEBASE_RADIUS, new ReplanningConfig()), + false, this); } diff --git a/src/main/java/swervelib/SwerveController.java b/src/main/java/swervelib/SwerveController.java new file mode 100644 index 00000000..0bb1fa7f --- /dev/null +++ b/src/main/java/swervelib/SwerveController.java @@ -0,0 +1,234 @@ +package swervelib; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import swervelib.parser.SwerveControllerConfiguration; + +/** + * Controller class used to convert raw inputs into robot speeds. + */ +public class SwerveController +{ + + /** + * {@link SwerveControllerConfiguration} object storing data to generate the {@link PIDController} for controlling the + * robot heading, and deadband for heading joystick. + */ + public final SwerveControllerConfiguration config; + /** + * PID Controller for the robot heading. + */ + public final PIDController thetaController; // TODO: Switch to ProfilePIDController + /** + * Last angle as a scalar [-1,1] the robot was set to. + */ + public double lastAngleScalar; + /** + * {@link SlewRateLimiter} for movement in the X direction in meters/second. + */ + public SlewRateLimiter xLimiter = null; + /** + * {@link SlewRateLimiter} for movement in the Y direction in meters/second. + */ + public SlewRateLimiter yLimiter = null; + /** + * {@link SlewRateLimiter} for angular movement in radians/second. + */ + public SlewRateLimiter angleLimiter = null; + + /** + * Construct the SwerveController object which is used for determining the speeds of the robot based on controller + * input. + * + * @param cfg {@link SwerveControllerConfiguration} containing the PIDF variables for the heading PIDF. + */ + public SwerveController(SwerveControllerConfiguration cfg) + { + config = cfg; + thetaController = config.headingPIDF.createPIDController(); + thetaController.enableContinuousInput(-Math.PI, Math.PI); + lastAngleScalar = 0; + } + + /** + * Helper function to get the {@link Translation2d} of the chassis speeds given the {@link ChassisSpeeds}. + * + * @param speeds Chassis speeds. + * @return {@link Translation2d} of the speed the robot is going in. + */ + public static Translation2d getTranslation2d(ChassisSpeeds speeds) + { + return new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); + } + + /** + * Add slew rate limiters to all controls. This prevents the robot from ramping up too much. To disable a + * {@link SlewRateLimiter} set the desired one to null. + * + * @param x The {@link SlewRateLimiter} for the X velocity in meters/second. + * @param y The {@link SlewRateLimiter} for the Y velocity in meters/second. + * @param angle The {@link SlewRateLimiter} for the angular velocity in radians/second. + */ + public void addSlewRateLimiters(SlewRateLimiter x, SlewRateLimiter y, SlewRateLimiter angle) + { + xLimiter = x; + yLimiter = y; + angleLimiter = angle; + } + + /** + * Calculate the hypot deadband and check if the joystick is within it. + * + * @param x The x value for the joystick in which the deadband should be applied. + * @param y The y value for the joystick in which the deadband should be applied. + * @return Whether the values are within the deadband from + * {@link SwerveControllerConfiguration#angleJoyStickRadiusDeadband}. + */ + public boolean withinHypotDeadband(double x, double y) + { + return Math.hypot(x, y) < config.angleJoyStickRadiusDeadband; + } + + /** + * Get the chassis speeds based on controller input of 1 joystick [-1,1] and an angle. + * + * @param xInput X joystick input for the robot to move in the X direction. X = xInput * maxSpeed + * @param yInput Y joystick input for the robot to move in the Y direction. Y = yInput * + * maxSpeed; + * @param angle The desired angle of the robot in radians. + * @param currentHeadingAngleRadians The current robot heading in radians. + * @param maxSpeed Maximum speed in meters per second. + * @return {@link ChassisSpeeds} which can be sent to th Swerve Drive. + */ + public ChassisSpeeds getTargetSpeeds( + double xInput, double yInput, double angle, double currentHeadingAngleRadians, double maxSpeed) + { + // Convert joystick inputs to m/s by scaling by max linear speed. Also uses a cubic function + // to allow for precise control and fast movement. + double x = xInput * maxSpeed; + double y = yInput * maxSpeed; + + return getRawTargetSpeeds(x, y, angle, currentHeadingAngleRadians); + } + + /** + * Get the angle in radians based off of the heading joysticks. + * + * @param headingX X joystick which controls the angle of the robot. + * @param headingY Y joystick which controls the angle of the robot. + * @return angle in radians from the joystick. + */ + public double getJoystickAngle(double headingX, double headingY) + { + lastAngleScalar = + withinHypotDeadband(headingX, headingY) ? lastAngleScalar : Math.atan2(headingX, headingY); + return lastAngleScalar; + } + + /** + * Get the chassis speeds based on controller input of 2 joysticks. One for speeds in which direction. The other for + * the angle of the robot. + * + * @param xInput X joystick input for the robot to move in the X direction. + * @param yInput Y joystick input for the robot to move in the Y direction. + * @param headingX X joystick which controls the angle of the robot. + * @param headingY Y joystick which controls the angle of the robot. + * @param currentHeadingAngleRadians The current robot heading in radians. + * @param maxSpeed Maximum speed of the drive motors in meters per second, multiplier of the xInput + * and yInput. + * @return {@link ChassisSpeeds} which can be sent to th Swerve Drive. + */ + public ChassisSpeeds getTargetSpeeds( + double xInput, + double yInput, + double headingX, + double headingY, + double currentHeadingAngleRadians, + double maxSpeed) + { + // Converts the horizontal and vertical components to the commanded angle, in radians, unless + // the joystick is near + // the center (i. e. has been released), in which case the angle is held at the last valid + // joystick input (hold + // position when stick released). + double angle = + withinHypotDeadband(headingX, headingY) ? lastAngleScalar : Math.atan2(headingX, headingY); + ChassisSpeeds speeds = getTargetSpeeds(xInput, yInput, angle, currentHeadingAngleRadians, maxSpeed); + + // Used for the position hold feature + lastAngleScalar = angle; + + return speeds; + } + + /** + * Get the {@link ChassisSpeeds} based of raw speeds desired in meters/second and heading in radians. + * + * @param xSpeed X speed in meters per second. + * @param ySpeed Y speed in meters per second. + * @param omega Angular velocity in radians/second. + * @return {@link ChassisSpeeds} the robot should move to. + */ + public ChassisSpeeds getRawTargetSpeeds(double xSpeed, double ySpeed, double omega) + { + if (xLimiter != null) + { + xSpeed = xLimiter.calculate(xSpeed); + } + if (yLimiter != null) + { + ySpeed = yLimiter.calculate(ySpeed); + } + if (angleLimiter != null) + { + omega = angleLimiter.calculate(omega); + } + + return new ChassisSpeeds(xSpeed, ySpeed, omega); + } + + /** + * Get the {@link ChassisSpeeds} based of raw speeds desired in meters/second and heading in radians. + * + * @param xSpeed X speed in meters per second. + * @param ySpeed Y speed in meters per second. + * @param targetHeadingAngleRadians Target heading in radians. + * @param currentHeadingAngleRadians Current heading in radians. + * @return {@link ChassisSpeeds} the robot should move to. + */ + public ChassisSpeeds getRawTargetSpeeds(double xSpeed, double ySpeed, double targetHeadingAngleRadians, + double currentHeadingAngleRadians) + { + // Calculates an angular rate using a PIDController and the commanded angle. Returns a value between -1 and 1 + // which is then scaled to be between -maxAngularVelocity and +maxAngularVelocity. + return getRawTargetSpeeds(xSpeed, ySpeed, + thetaController.calculate(currentHeadingAngleRadians, targetHeadingAngleRadians) * + config.maxAngularVelocity); + } + + /** + * Calculate the angular velocity given the current and target heading angle in radians. + * + * @param currentHeadingAngleRadians The current heading of the robot in radians. + * @param targetHeadingAngleRadians The target heading of the robot in radians. + * @return Angular velocity in radians per second. + */ + public double headingCalculate(double currentHeadingAngleRadians, double targetHeadingAngleRadians) + { + return thetaController.calculate(currentHeadingAngleRadians, targetHeadingAngleRadians) * config.maxAngularVelocity; + } + + /** + * Set a new maximum angular velocity that is different from the auto-generated one. Modified the + * {@link SwerveControllerConfiguration#maxAngularVelocity} field which is used in the {@link SwerveController} class + * for {@link ChassisSpeeds} generation. + * + * @param angularVelocity Angular velocity in radians per second. + */ + public void setMaximumAngularVelocity(double angularVelocity) + { + config.maxAngularVelocity = angularVelocity; + } +} diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java new file mode 100644 index 00000000..ec7f452d --- /dev/null +++ b/src/main/java/swervelib/SwerveDrive.java @@ -0,0 +1,1082 @@ +package swervelib; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; +import swervelib.imu.SwerveIMU; +import swervelib.math.SwerveMath; +import swervelib.parser.SwerveControllerConfiguration; +import swervelib.parser.SwerveDriveConfiguration; +import swervelib.simulation.SwerveIMUSimulation; +import swervelib.telemetry.SwerveDriveTelemetry; +import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; + +/** + * Swerve Drive class representing and controlling the swerve drive. + */ +public class SwerveDrive +{ + + /** + * Swerve Kinematics object. + */ + public final SwerveDriveKinematics kinematics; + /** + * Swerve drive configuration. + */ + public final SwerveDriveConfiguration swerveDriveConfiguration; + /** + * Swerve odometry. + */ + public final SwerveDrivePoseEstimator swerveDrivePoseEstimator; + /** + * Swerve modules. + */ + private final SwerveModule[] swerveModules; + /** + * WPILib {@link Notifier} to keep odometry up to date. + */ + private final Notifier odometryThread; + /** + * Odometry lock to ensure thread safety. + */ + private final Lock odometryLock = new ReentrantLock(); + /** + * Field object. + */ + public Field2d field = new Field2d(); + /** + * Swerve controller for controlling heading of the robot. + */ + public SwerveController swerveController; + /** + * Standard deviation of encoders and gyroscopes, usually should not change. (meters of position and degrees of + * rotation) + */ + public Matrix stateStdDevs = VecBuilder.fill(0.1, + 0.1, + 0.1); + /** + * The standard deviation of the vision measurement, for best accuracy calculate the standard deviation at 2 or more + * points and fit a line to it and modify this using {@link SwerveDrive#addVisionMeasurement(Pose2d, double, Matrix)} + * with the calculated optimal standard deviation. (Units should be meters per pixel). By optimizing this you can get + * vision accurate to inches instead of feet. + */ + public Matrix visionMeasurementStdDevs = VecBuilder.fill(0.9, + 0.9, + 0.9); + /** + * Invert odometry readings of drive motor positions, used as a patch for debugging currently. + */ + public boolean invertOdometry = false; + /** + * Correct chassis velocity in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)} using 254's + * correction. + */ + public boolean chassisVelocityCorrection = true; + /** + * Whether to correct heading when driving translationally. Set to true to enable. + */ + public boolean headingCorrection = false; + /** + * Swerve IMU device for sensing the heading of the robot. + */ + private SwerveIMU imu; + /** + * Simulation of the swerve drive. + */ + private SwerveIMUSimulation simIMU; + /** + * Counter to synchronize the modules relative encoder with absolute encoder when not moving. + */ + private int moduleSynchronizationCounter = 0; + /** + * The last heading set in radians. + */ + private double lastHeadingRadians = 0; + /** + * The absolute max speed that your robot can reach while translating in meters per second. + */ + private double attainableMaxTranslationalSpeedMetersPerSecond = 0; + /** + * The absolute max speed the robot can reach while rotating radians per second. + */ + private double attainableMaxRotationalVelocityRadiansPerSecond = 0; + /** + * Maximum speed of the robot in meters per second. + */ + private double maxSpeedMPS; + + /** + * Creates a new swerve drivebase subsystem. Robot is controlled via the {@link SwerveDrive#drive} method, or via the + * {@link SwerveDrive#setRawModuleStates} method. The {@link SwerveDrive#drive} method incorporates kinematics-- it + * takes a translation and rotation, as well as parameters for field-centric and closed-loop velocity control. + * {@link SwerveDrive#setRawModuleStates} takes a list of SwerveModuleStates and directly passes them to the modules. + * This subsystem also handles odometry. + * + * @param config The {@link SwerveDriveConfiguration} configuration to base the swerve drive off of. + * @param controllerConfig The {@link SwerveControllerConfiguration} to use when creating the + * {@link SwerveController}. + * @param maxSpeedMPS Maximum speed in meters per second, remember to use {@link Units#feetToMeters(double)} if + * you have feet per second! + */ + public SwerveDrive( + SwerveDriveConfiguration config, SwerveControllerConfiguration controllerConfig, double maxSpeedMPS) + { + this.maxSpeedMPS = maxSpeedMPS; + swerveDriveConfiguration = config; + swerveController = new SwerveController(controllerConfig); + // Create Kinematics from swerve module locations. + kinematics = new SwerveDriveKinematics(config.moduleLocationsMeters); + odometryThread = new Notifier(this::updateOdometry); + + // Create an integrator for angle if the robot is being simulated to emulate an IMU + // If the robot is real, instantiate the IMU instead. + if (SwerveDriveTelemetry.isSimulation) + { + simIMU = new SwerveIMUSimulation(); + } else + { + imu = config.imu; + imu.factoryDefault(); + } + + this.swerveModules = config.modules; + + // odometry = new SwerveDriveOdometry(kinematics, getYaw(), getModulePositions()); + swerveDrivePoseEstimator = + new SwerveDrivePoseEstimator( + kinematics, + getYaw(), + getModulePositions(), + new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(0)), + stateStdDevs, + visionMeasurementStdDevs); // x,y,heading in radians; Vision measurement std dev, higher=less weight + + zeroGyro(); + + // Initialize Telemetry + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal()) + { + SmartDashboard.putData("Field", field); + } + + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) + { + SwerveDriveTelemetry.maxSpeed = maxSpeedMPS; + SwerveDriveTelemetry.maxAngularVelocity = swerveController.config.maxAngularVelocity; + SwerveDriveTelemetry.moduleCount = swerveModules.length; + SwerveDriveTelemetry.sizeFrontBack = Units.metersToInches(SwerveMath.getSwerveModule(swerveModules, true, + false).moduleLocation.getX() + + SwerveMath.getSwerveModule(swerveModules, + false, + false).moduleLocation.getX()); + SwerveDriveTelemetry.sizeLeftRight = Units.metersToInches(SwerveMath.getSwerveModule(swerveModules, false, + true).moduleLocation.getY() + + SwerveMath.getSwerveModule(swerveModules, + false, + false).moduleLocation.getY()); + SwerveDriveTelemetry.wheelLocations = new double[SwerveDriveTelemetry.moduleCount * 2]; + for (SwerveModule module : swerveModules) + { + SwerveDriveTelemetry.wheelLocations[module.moduleNumber * 2] = Units.metersToInches( + module.configuration.moduleLocation.getX()); + SwerveDriveTelemetry.wheelLocations[(module.moduleNumber * 2) + 1] = Units.metersToInches( + module.configuration.moduleLocation.getY()); + } + SwerveDriveTelemetry.measuredStates = new double[SwerveDriveTelemetry.moduleCount * 2]; + SwerveDriveTelemetry.desiredStates = new double[SwerveDriveTelemetry.moduleCount * 2]; + } + + odometryThread.startPeriodic(SwerveDriveTelemetry.isSimulation ? 0.01 : 0.02); + } + + /** + * Set the odometry update period in seconds. + * + * @param period period in seconds. + */ + public void setOdometryPeriod(double period) + { + odometryThread.stop(); + odometryThread.startPeriodic(period); + } + + /** + * Stop the odometry thread in favor of manually updating odometry. + */ + public void stopOdometryThread() + { + odometryThread.stop(); + } + + /** + * Set the conversion factor for the angle/azimuth motor controller. + * + * @param conversionFactor Angle motor conversion factor for PID, should be generated from + * {@link SwerveMath#calculateDegreesPerSteeringRotation(double, double)} or calculated. + */ + public void setAngleMotorConversionFactor(double conversionFactor) + { + for (SwerveModule module : swerveModules) + { + module.setAngleMotorConversionFactor(conversionFactor); + } + } + + /** + * Set the conversion factor for the drive motor controller. + * + * @param conversionFactor Drive motor conversion factor for PID, should be generated from + * {@link SwerveMath#calculateMetersPerRotation(double, double, double)} or calculated. + */ + public void setDriveMotorConversionFactor(double conversionFactor) + { + for (SwerveModule module : swerveModules) + { + module.setDriveMotorConversionFactor(conversionFactor); + } + } + + /** + * Set the heading correction capabilities of YAGSL. + * + * @param state {@link SwerveDrive#headingCorrection} state. + */ + public void setHeadingCorrection(boolean state) + { + headingCorrection = state; + } + + /** + * Secondary method of controlling the drive base given velocity and adjusting it for field oriented use. + * + * @param velocity Velocity of the robot desired. + */ + public void driveFieldOriented(ChassisSpeeds velocity) + { + ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getYaw()); + drive(fieldOrientedVelocity); + } + + /** + * Secondary method of controlling the drive base given velocity and adjusting it for field oriented use. + * + * @param velocity Velocity of the robot desired. + * @param centerOfRotationMeters The center of rotation in meters, 0 is the center of the robot. + */ + public void driveFieldOriented(ChassisSpeeds velocity, Translation2d centerOfRotationMeters) + { + ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getYaw()); + drive(fieldOrientedVelocity, centerOfRotationMeters); + } + + /** + * Secondary method for controlling the drivebase. Given a simple {@link ChassisSpeeds} set the swerve module states, + * to achieve the goal. + * + * @param velocity The desired robot-oriented {@link ChassisSpeeds} for the robot to achieve. + */ + public void drive(ChassisSpeeds velocity) + { + drive(velocity, false, new Translation2d()); + } + + /** + * Secondary method for controlling the drivebase. Given a simple {@link ChassisSpeeds} set the swerve module states, + * to achieve the goal. + * + * @param velocity The desired robot-oriented {@link ChassisSpeeds} for the robot to achieve. + * @param centerOfRotationMeters The center of rotation in meters, 0 is the center of the robot. + */ + public void drive(ChassisSpeeds velocity, Translation2d centerOfRotationMeters) + { + drive(velocity, false, centerOfRotationMeters); + } + + /** + * The primary method for controlling the drivebase. Takes a {@link Translation2d} and a rotation rate, and calculates + * and commands module states accordingly. Can use either open-loop or closed-loop velocity control for the wheel + * velocities. Also has field- and robot-relative modes, which affect how the translation vector is used. + * + * @param translation {@link Translation2d} that is the commanded linear velocity of the robot, in meters + * per second. In robot-relative mode, positive x is torwards the bow (front) and + * positive y is torwards port (left). In field-relative mode, positive x is away from + * the alliance wall (field North) and positive y is torwards the left wall when looking + * through the driver station glass (field West). + * @param rotation Robot angular rate, in radians per second. CCW positive. Unaffected by field/robot + * relativity. + * @param fieldRelative Drive mode. True for field-relative, false for robot-relative. + * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. + * @param centerOfRotationMeters The center of rotation in meters, 0 is the center of the robot. + */ + public void drive( + Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop, + Translation2d centerOfRotationMeters) + { + // Creates a robot-relative ChassisSpeeds object, converting from field-relative speeds if + // necessary. + ChassisSpeeds velocity = + fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds( + translation.getX(), translation.getY(), rotation, getYaw()) + : new ChassisSpeeds(translation.getX(), translation.getY(), rotation); + + drive(velocity, isOpenLoop, centerOfRotationMeters); + } + + /** + * The primary method for controlling the drivebase. Takes a {@link Translation2d} and a rotation rate, and calculates + * and commands module states accordingly. Can use either open-loop or closed-loop velocity control for the wheel + * velocities. Also has field- and robot-relative modes, which affect how the translation vector is used. + * + * @param translation {@link Translation2d} that is the commanded linear velocity of the robot, in meters per + * second. In robot-relative mode, positive x is torwards the bow (front) and positive y is + * torwards port (left). In field-relative mode, positive x is away from the alliance wall (field + * North) and positive y is torwards the left wall when looking through the driver station glass + * (field West). + * @param rotation Robot angular rate, in radians per second. CCW positive. Unaffected by field/robot + * relativity. + * @param fieldRelative Drive mode. True for field-relative, false for robot-relative. + * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. + */ + public void drive( + Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) + { + // Creates a robot-relative ChassisSpeeds object, converting from field-relative speeds if + // necessary. + ChassisSpeeds velocity = + fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds( + translation.getX(), translation.getY(), rotation, getYaw()) + : new ChassisSpeeds(translation.getX(), translation.getY(), rotation); + + drive(velocity, isOpenLoop, new Translation2d()); + } + + /** + * The primary method for controlling the drivebase. Takes a {@link ChassisSpeeds}, and calculates and commands module + * states accordingly. Can use either open-loop or closed-loop velocity control for the wheel velocities. Also has + * field- and robot-relative modes, which affect how the translation vector is used. + * + * @param velocity The chassis speeds to set the robot to achieve. + * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. + * @param centerOfRotationMeters The center of rotation in meters, 0 is the center of the robot. + */ + public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d centerOfRotationMeters) + { + + // Thank you to Jared Russell FRC254 for Open Loop Compensation Code + // https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5 + if (chassisVelocityCorrection) + { + velocity = ChassisSpeeds.discretize(velocity, 0.02); + } + + // Heading Angular Velocity Deadband, might make a configuration option later. + // Originally made by Team 1466 Webb Robotics. + if (headingCorrection) + { + if (Math.abs(velocity.omegaRadiansPerSecond) < 0.01) + { + velocity.omegaRadiansPerSecond = + swerveController.headingCalculate(lastHeadingRadians, getYaw().getRadians()); + } else + { + lastHeadingRadians = getYaw().getRadians(); + } + } + + // Display commanded speed for testing + if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) + { + SmartDashboard.putString("RobotVelocity", velocity.toString()); + } + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) + { + SwerveDriveTelemetry.desiredChassisSpeeds[1] = velocity.vyMetersPerSecond; + SwerveDriveTelemetry.desiredChassisSpeeds[0] = velocity.vxMetersPerSecond; + SwerveDriveTelemetry.desiredChassisSpeeds[2] = Math.toDegrees(velocity.omegaRadiansPerSecond); + } + + // Calculate required module states via kinematics + SwerveModuleState[] swerveModuleStates = kinematics.toSwerveModuleStates(velocity, centerOfRotationMeters); + + setRawModuleStates(swerveModuleStates, isOpenLoop); + } + + + /** + * Set the maximum speeds for desaturation. + * + * @param attainableMaxModuleSpeedMetersPerSecond The absolute max speed that a module can reach in meters per + * second. + * @param attainableMaxTranslationalSpeedMetersPerSecond The absolute max speed that your robot can reach while + * translating in meters per second. + * @param attainableMaxRotationalVelocityRadiansPerSecond The absolute max speed the robot can reach while rotating in + * radians per second. + */ + public void setMaximumSpeeds( + double attainableMaxModuleSpeedMetersPerSecond, + double attainableMaxTranslationalSpeedMetersPerSecond, + double attainableMaxRotationalVelocityRadiansPerSecond) + { + setMaximumSpeed(attainableMaxModuleSpeedMetersPerSecond); + this.attainableMaxTranslationalSpeedMetersPerSecond = attainableMaxTranslationalSpeedMetersPerSecond; + this.attainableMaxRotationalVelocityRadiansPerSecond = attainableMaxRotationalVelocityRadiansPerSecond; + } + + /** + * Set the module states (azimuth and velocity) directly. Used primarily for auto pathing. + * + * @param desiredStates A list of SwerveModuleStates to send to the modules. + * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. + */ + private void setRawModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop) + { + // Desaturates wheel speeds + if (attainableMaxTranslationalSpeedMetersPerSecond != 0 || attainableMaxRotationalVelocityRadiansPerSecond != 0) + { + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, getRobotVelocity(), + maxSpeedMPS, + attainableMaxTranslationalSpeedMetersPerSecond, + attainableMaxRotationalVelocityRadiansPerSecond); + } + + // Sets states + for (SwerveModule module : swerveModules) + { + module.setDesiredState(desiredStates[module.moduleNumber], isOpenLoop, false); + + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) + { + SwerveDriveTelemetry.desiredStates[module.moduleNumber * + 2] = module.lastState.angle.getDegrees(); + SwerveDriveTelemetry.desiredStates[(module.moduleNumber * 2) + + 1] = module.lastState.speedMetersPerSecond; + } + } + } + + /** + * Set the module states (azimuth and velocity) directly. Used primarily for auto paths. + * + * @param desiredStates A list of SwerveModuleStates to send to the modules. + * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. + */ + public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop) + { + setRawModuleStates(kinematics.toSwerveModuleStates(kinematics.toChassisSpeeds(desiredStates)), + isOpenLoop); + } + + /** + * Set chassis speeds with closed-loop velocity control. + * + * @param chassisSpeeds Chassis speeds to set. + */ + public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) + { + SwerveDriveTelemetry.desiredChassisSpeeds[1] = chassisSpeeds.vyMetersPerSecond; + SwerveDriveTelemetry.desiredChassisSpeeds[0] = chassisSpeeds.vxMetersPerSecond; + SwerveDriveTelemetry.desiredChassisSpeeds[2] = Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond); + + setRawModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds), false); + } + + /** + * Gets the current pose (position and rotation) of the robot, as reported by odometry. + * + * @return The robot's pose + */ + public Pose2d getPose() + { + + odometryLock.lock(); + Pose2d poseEstimation = swerveDrivePoseEstimator.getEstimatedPosition(); + odometryLock.unlock(); + return poseEstimation; + } + + /** + * Gets the current field-relative velocity (x, y and omega) of the robot + * + * @return A ChassisSpeeds object of the current field-relative velocity + */ + public ChassisSpeeds getFieldVelocity() + { + // ChassisSpeeds has a method to convert from field-relative to robot-relative speeds, + // but not the reverse. However, because this transform is a simple rotation, negating the + // angle + // given as the robot angle reverses the direction of rotation, and the conversion is reversed. + return ChassisSpeeds.fromFieldRelativeSpeeds( + kinematics.toChassisSpeeds(getStates()), getYaw().unaryMinus()); + } + + /** + * Gets the current robot-relative velocity (x, y and omega) of the robot + * + * @return A ChassisSpeeds object of the current robot-relative velocity + */ + public ChassisSpeeds getRobotVelocity() + { + return kinematics.toChassisSpeeds(getStates()); + } + + /** + * Resets odometry to the given pose. Gyro angle and module positions do not need to be reset when calling this + * method. However, if either gyro angle or module position is reset, this must be called in order for odometry to + * keep working. + * + * @param pose The pose to set the odometry to + */ + public void resetOdometry(Pose2d pose) + { + odometryLock.lock(); + swerveDrivePoseEstimator.resetPosition(pose.getRotation(), getModulePositions(), pose); + odometryLock.unlock(); + kinematics.toSwerveModuleStates(ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, 0, pose.getRotation())); + } + + /** + * Post the trajectory to the field + * + * @param trajectory the trajectory to post. + */ + public void postTrajectory(Trajectory trajectory) + { + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal()) + { + field.getObject("Trajectory").setTrajectory(trajectory); + } + } + + /** + * Gets the current module states (azimuth and velocity) + * + * @return A list of SwerveModuleStates containing the current module states + */ + public SwerveModuleState[] getStates() + { + SwerveModuleState[] states = new SwerveModuleState[swerveDriveConfiguration.moduleCount]; + for (SwerveModule module : swerveModules) + { + states[module.moduleNumber] = module.getState(); + } + return states; + } + + /** + * Gets the current module positions (azimuth and wheel position (meters)). Inverts the distance from each module if + * {@link #invertOdometry} is true. + * + * @return A list of SwerveModulePositions containg the current module positions + */ + public SwerveModulePosition[] getModulePositions() + { + SwerveModulePosition[] positions = + new SwerveModulePosition[swerveDriveConfiguration.moduleCount]; + for (SwerveModule module : swerveModules) + { + positions[module.moduleNumber] = module.getPosition(); + if (invertOdometry) + { + positions[module.moduleNumber].distanceMeters *= -1; + } + } + return positions; + } + + /** + * Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0. + */ + public void zeroGyro() + { + // Resets the real gyro or the angle accumulator, depending on whether the robot is being + // simulated + if (!SwerveDriveTelemetry.isSimulation) + { + imu.setOffset(imu.getRawRotation3d()); + } else + { + simIMU.setAngle(0); + } + swerveController.lastAngleScalar = 0; + lastHeadingRadians = 0; + resetOdometry(new Pose2d(getPose().getTranslation(), new Rotation2d())); + } + + /** + * Gets the current yaw angle of the robot, as reported by the imu. CCW positive, not wrapped. + * + * @return The yaw as a {@link Rotation2d} angle + */ + public Rotation2d getYaw() + { + // Read the imu if the robot is real or the accumulator if the robot is simulated. + if (!SwerveDriveTelemetry.isSimulation) + { + return swerveDriveConfiguration.invertedIMU + ? Rotation2d.fromRadians(imu.getRotation3d().unaryMinus().getZ()) + : Rotation2d.fromRadians(imu.getRotation3d().getZ()); + } else + { + return simIMU.getYaw(); + } + } + + /** + * Gets the current pitch angle of the robot, as reported by the imu. + * + * @return The heading as a {@link Rotation2d} angle + */ + public Rotation2d getPitch() + { + // Read the imu if the robot is real or the accumulator if the robot is simulated. + if (!SwerveDriveTelemetry.isSimulation) + { + return swerveDriveConfiguration.invertedIMU + ? Rotation2d.fromRadians(imu.getRotation3d().unaryMinus().getY()) + : Rotation2d.fromRadians(imu.getRotation3d().getY()); + } else + { + return simIMU.getPitch(); + } + } + + /** + * Gets the current roll angle of the robot, as reported by the imu. + * + * @return The heading as a {@link Rotation2d} angle + */ + public Rotation2d getRoll() + { + // Read the imu if the robot is real or the accumulator if the robot is simulated. + if (!SwerveDriveTelemetry.isSimulation) + { + return swerveDriveConfiguration.invertedIMU + ? Rotation2d.fromRadians(imu.getRotation3d().unaryMinus().getX()) + : Rotation2d.fromRadians(imu.getRotation3d().getX()); + } else + { + return simIMU.getRoll(); + } + } + + /** + * Gets the current gyro {@link Rotation3d} of the robot, as reported by the imu. + * + * @return The heading as a {@link Rotation3d} angle + */ + public Rotation3d getGyroRotation3d() + { + // Read the imu if the robot is real or the accumulator if the robot is simulated. + if (!SwerveDriveTelemetry.isSimulation) + { + return swerveDriveConfiguration.invertedIMU + ? imu.getRotation3d().unaryMinus() + : imu.getRotation3d(); + } else + { + return simIMU.getGyroRotation3d(); + } + } + + /** + * Gets current acceleration of the robot in m/s/s. If gyro unsupported returns empty. + * + * @return acceleration of the robot as a {@link Translation3d} + */ + public Optional getAccel() + { + if (!SwerveDriveTelemetry.isSimulation) + { + return imu.getAccel(); + } else + { + return simIMU.getAccel(); + } + } + + /** + * Sets the drive motors to brake/coast mode. + * + * @param brake True to set motors to brake mode, false for coast. + */ + public void setMotorIdleMode(boolean brake) + { + for (SwerveModule swerveModule : swerveModules) + { + swerveModule.setMotorBrake(brake); + } + } + + /** + * Set the maximum speed of the drive motors, modified {@link SwerveDrive#maxSpeedMPS} which is used for the + * {@link SwerveDrive#setRawModuleStates(SwerveModuleState[], boolean)} function and + * {@link SwerveController#getTargetSpeeds(double, double, double, double, double)} functions. This function overrides + * what was placed in the JSON and could damage your motor/robot if set too high or unachievable rates. + * + * @param maximumSpeed Maximum speed for the drive motors in meters / second. + * @param updateModuleFeedforward Update the swerve module feedforward to account for the new maximum speed. This + * should be true unless you have replaced the drive motor feedforward with + * {@link SwerveDrive#replaceSwerveModuleFeedforward(SimpleMotorFeedforward)} + * @param optimalVoltage Optimal voltage to use for the feedforward. + */ + public void setMaximumSpeed(double maximumSpeed, boolean updateModuleFeedforward, double optimalVoltage) + { + maxSpeedMPS = maximumSpeed; + swerveDriveConfiguration.physicalCharacteristics.optimalVoltage = optimalVoltage; + for (SwerveModule module : swerveModules) + { + if (updateModuleFeedforward) + { + module.feedforward = SwerveMath.createDriveFeedforward(optimalVoltage, + maximumSpeed, + swerveDriveConfiguration.physicalCharacteristics.wheelGripCoefficientOfFriction); + } + } + } + + /** + * Set the maximum speed of the drive motors, modified {@link SwerveDrive#maxSpeedMPS} which is used for the + * {@link SwerveDrive#setRawModuleStates(SwerveModuleState[], boolean)} function and + * {@link SwerveController#getTargetSpeeds(double, double, double, double, double)} functions. This function overrides + * what was placed in the JSON and could damage your motor/robot if set too high or unachievable rates. Overwrites the + * {@link SwerveModule#feedforward}. + * + * @param maximumSpeed Maximum speed for the drive motors in meters / second. + */ + public void setMaximumSpeed(double maximumSpeed) + { + setMaximumSpeed(maximumSpeed, true, swerveDriveConfiguration.physicalCharacteristics.optimalVoltage); + } + + /** + * Point all modules toward the robot center, thus making the robot very difficult to move. Forcing the robot to keep + * the current pose. + */ + public void lockPose() + { + // Sets states + for (SwerveModule swerveModule : swerveModules) + { + SwerveModuleState desiredState = + new SwerveModuleState(0, swerveModule.configuration.moduleLocation.getAngle()); + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) + { + SwerveDriveTelemetry.desiredStates[swerveModule.moduleNumber * 2] = + desiredState.angle.getDegrees(); + SwerveDriveTelemetry.desiredStates[(swerveModule.moduleNumber * 2) + 1] = + desiredState.speedMetersPerSecond; + } + swerveModule.setDesiredState(desiredState, false, true); + + } + + // Update kinematics because we are not using setModuleStates + kinematics.toSwerveModuleStates(new ChassisSpeeds()); + } + + /** + * Get the swerve module poses and on the field relative to the robot. + * + * @param robotPose Robot pose. + * @return Swerve module poses. + */ + public Pose2d[] getSwerveModulePoses(Pose2d robotPose) + { + Pose2d[] poseArr = new Pose2d[swerveDriveConfiguration.moduleCount]; + List poses = new ArrayList<>(); + for (SwerveModule module : swerveModules) + { + poses.add( + robotPose.plus( + new Transform2d(module.configuration.moduleLocation, module.getState().angle))); + } + return poses.toArray(poseArr); + } + + /** + * Setup the swerve module feedforward. + * + * @param feedforward Feedforward for the drive motor on swerve modules. + */ + public void replaceSwerveModuleFeedforward(SimpleMotorFeedforward feedforward) + { + for (SwerveModule swerveModule : swerveModules) + { + swerveModule.feedforward = feedforward; + } + } + + /** + * Update odometry should be run every loop. Synchronizes module absolute encoders with relative encoders + * periodically. In simulation mode will also post the pose of each module. Updates SmartDashboard with module encoder + * readings and states. + */ + public void updateOdometry() + { + odometryLock.lock(); + try + { + // Update odometry + swerveDrivePoseEstimator.update(getYaw(), getModulePositions()); + + // Update angle accumulator if the robot is simulated + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) + { + Pose2d[] modulePoses = getSwerveModulePoses(swerveDrivePoseEstimator.getEstimatedPosition()); + if (SwerveDriveTelemetry.isSimulation) + { + simIMU.updateOdometry( + kinematics, + getStates(), + modulePoses, + field); + } + + ChassisSpeeds measuredChassisSpeeds = getRobotVelocity(); + SwerveDriveTelemetry.measuredChassisSpeeds[1] = measuredChassisSpeeds.vyMetersPerSecond; + SwerveDriveTelemetry.measuredChassisSpeeds[0] = measuredChassisSpeeds.vxMetersPerSecond; + SwerveDriveTelemetry.measuredChassisSpeeds[2] = Math.toDegrees(measuredChassisSpeeds.omegaRadiansPerSecond); + SwerveDriveTelemetry.robotRotation = getYaw().getDegrees(); + } + + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal()) + { + field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition()); + } + + double sumVelocity = 0; + for (SwerveModule module : swerveModules) + { + SwerveModuleState moduleState = module.getState(); + sumVelocity += Math.abs(moduleState.speedMetersPerSecond); + if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) + { + SmartDashboard.putNumber( + "Module[" + module.configuration.name + "] Relative Encoder", module.getRelativePosition()); + SmartDashboard.putNumber( + "Module[" + module.configuration.name + "] Absolute Encoder", module.getAbsolutePosition()); + SmartDashboard.putNumber( + "Module[" + module.configuration.name + "] Absolute Encoder Read Issue", + module.getAbsoluteEncoderReadIssue() ? 1 : 0); + } + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) + { + SwerveDriveTelemetry.measuredStates[module.moduleNumber * 2] = moduleState.angle.getDegrees(); + SwerveDriveTelemetry.measuredStates[(module.moduleNumber * 2) + 1] = moduleState.speedMetersPerSecond; + } + } + + // If the robot isn't moving synchronize the encoders every 100ms (Inspired by democrat's SDS + // lib) + // To ensure that everytime we initialize it works. + if (sumVelocity <= .01 && ++moduleSynchronizationCounter > 5) + { + synchronizeModuleEncoders(); + moduleSynchronizationCounter = 0; + } + + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) + { + SwerveDriveTelemetry.updateData(); + } + } catch (Exception e) + { + odometryLock.unlock(); + throw e; + } + odometryLock.unlock(); + } + + /** + * Synchronize angle motor integrated encoders with data from absolute encoders. + */ + public void synchronizeModuleEncoders() + { + for (SwerveModule module : swerveModules) + { + module.queueSynchronizeEncoders(); + } + } + + /** + * Set the gyro scope offset to a desired known rotation. Unlike {@link SwerveDrive#setGyro(Rotation3d)} it DOES NOT + * take the current rotation into account. + * + * @param offset {@link Rotation3d} known offset of the robot for gyroscope to use. + */ + public void setGyroOffset(Rotation3d offset) + { + if (SwerveDriveTelemetry.isSimulation) + { + simIMU.setAngle(offset.getZ()); + } else + { + imu.setOffset(offset); + } + } + + /** + * Add a vision measurement to the {@link SwerveDrivePoseEstimator} and update the {@link SwerveIMU} gyro reading with + * the given timestamp of the vision measurement.
IT IS HIGHLY RECOMMENDED TO UPDATE YOUR GYROSCOPE OFFSET + * AFTER USING THIS FUNCTION!
To update your gyroscope readings directly use + * {@link SwerveDrive#setGyroOffset(Rotation3d)}. + * + * @param robotPose Robot {@link Pose2d} as measured by vision. + * @param timestamp Timestamp the measurement was taken as time since startup, should be taken from + * {@link Timer#getFPGATimestamp()} or similar sources. + */ + public void addVisionMeasurement(Pose2d robotPose, double timestamp) + { + odometryLock.lock(); + swerveDrivePoseEstimator.addVisionMeasurement(robotPose, timestamp); + Pose2d newOdometry = new Pose2d(swerveDrivePoseEstimator.getEstimatedPosition().getTranslation(), + robotPose.getRotation()); + odometryLock.unlock(); + + setGyroOffset(new Rotation3d(0, 0, robotPose.getRotation().getRadians())); + resetOdometry(newOdometry); + } + + /** + * Add a vision measurement to the {@link SwerveDrivePoseEstimator} and update the {@link SwerveIMU} gyro reading with + * the given timestamp of the vision measurement. + * + * @param robotPose Robot {@link Pose2d} as measured by vision. + * @param timestamp Timestamp the measurement was taken as time since startup, should be taken from + * {@link Timer#getFPGATimestamp()} or similar sources. + * @param visionMeasurementStdDevs Vision measurement standard deviation that will be sent to the + * {@link SwerveDrivePoseEstimator}.The standard deviation of the vision measurement, + * for best accuracy calculate the standard deviation at 2 or more points and fit a + * line to it with the calculated optimal standard deviation. (Units should be meters + * per pixel). By optimizing this you can get * vision accurate to inches instead of + * feet. + */ + public void addVisionMeasurement(Pose2d robotPose, double timestamp, + Matrix visionMeasurementStdDevs) + { + this.visionMeasurementStdDevs = visionMeasurementStdDevs; + addVisionMeasurement(robotPose, timestamp); + } + + + /** + * Set the expected gyroscope angle using a {@link Rotation3d} object. To reset gyro, set to a new {@link Rotation3d} + * subtracted from the current gyroscopic readings {@link SwerveIMU#getRotation3d()}. + * + * @param gyro expected gyroscope angle as {@link Rotation3d}. + */ + public void setGyro(Rotation3d gyro) + { + if (SwerveDriveTelemetry.isSimulation) + { + setGyroOffset(simIMU.getGyroRotation3d().minus(gyro)); + } else + { + setGyroOffset(imu.getRawRotation3d().minus(gyro)); + } + } + + /** + * Helper function to get the {@link SwerveDrive#swerveController} for the {@link SwerveDrive} which can be used to + * generate {@link ChassisSpeeds} for the robot to orient it correctly given axis or angles, and apply + * {@link edu.wpi.first.math.filter.SlewRateLimiter} to given inputs. Important functions to look at are + * {@link SwerveController#getTargetSpeeds(double, double, double, double, double)}, + * {@link SwerveController#addSlewRateLimiters(SlewRateLimiter, SlewRateLimiter, SlewRateLimiter)}, + * {@link SwerveController#getRawTargetSpeeds(double, double, double)}. + * + * @return {@link SwerveController} for the {@link SwerveDrive}. + */ + public SwerveController getSwerveController() + { + return swerveController; + } + + /** + * Get the {@link SwerveModule}s associated with the {@link SwerveDrive}. + * + * @return {@link SwerveModule} array specified by configurations. + */ + public SwerveModule[] getModules() + { + return swerveDriveConfiguration.modules; + } + + /** + * Reset the drive encoders on the robot, useful when manually resetting the robot without a reboot, like in + * autonomous. + */ + public void resetEncoders() + { + for (SwerveModule module : swerveModules) + { + module.configuration.driveMotor.setPosition(0); + } + } + + /** + * Configure whether the {@link SwerveModuleState} will be optimized to prevent spinning more than 90deg. + * + * @param optimizationEnabled Whether the module optimization is enabled. + */ + public void setModuleStateOptimization(boolean optimizationEnabled) + { + for (SwerveModule module : swerveModules) + { + module.moduleStateOptimization = optimizationEnabled; + } + } + + /** + * Pushes the Absolute Encoder offsets to the Encoder or Motor Controller, depending on type. Also removes the + * internal offsets to prevent double offsetting. + */ + public void pushOffsetsToControllers() + { + for (SwerveModule module : swerveModules) + { + module.pushOffsetsToControllers(); + } + } + + /** + * Restores Internal YAGSL Encoder offsets and sets the Encoder stored offset back to 0 + */ + public void restoreInternalOffset() + { + for (SwerveModule module : swerveModules) + { + module.restoreInternalOffset(); + } + } + +} diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java new file mode 100644 index 00000000..999ccc16 --- /dev/null +++ b/src/main/java/swervelib/SwerveModule.java @@ -0,0 +1,437 @@ +package swervelib; + +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import swervelib.encoders.SwerveAbsoluteEncoder; +import swervelib.math.SwerveMath; +import swervelib.motors.SwerveMotor; +import swervelib.parser.SwerveModuleConfiguration; +import swervelib.simulation.SwerveModuleSimulation; +import swervelib.telemetry.SwerveDriveTelemetry; +import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; + +/** + * The Swerve Module class which represents and controls Swerve Modules for the swerve drive. + */ +public class SwerveModule +{ + + /** + * Swerve module configuration options. + */ + public final SwerveModuleConfiguration configuration; + /** + * Swerve Motors. + */ + private final SwerveMotor angleMotor, driveMotor; + /** + * Absolute encoder for swerve drive. + */ + private final SwerveAbsoluteEncoder absoluteEncoder; + /** + * Module number for kinematics, usually 0 to 3. front left -> front right -> back left -> back right. + */ + public int moduleNumber; + /** + * Feedforward for drive motor during closed loop control. + */ + public SimpleMotorFeedforward feedforward; + /** + * Maximum speed of the drive motors in meters per second. + */ + public double maxSpeed; + /** + * Last swerve module state applied. + */ + public SwerveModuleState lastState; + /** + * Enable {@link SwerveModuleState} optimizations so the angle is reversed and speed is reversed to ensure the module + * never turns more than 90deg. + */ + public boolean moduleStateOptimization = true; + /** + * Angle offset from the absolute encoder. + */ + private double angleOffset; + /** + * Simulated swerve module. + */ + private SwerveModuleSimulation simModule; + /** + * Encoder synchronization queued. + */ + private boolean synchronizeEncoderQueued = false; + + /** + * Construct the swerve module and initialize the swerve module motors and absolute encoder. + * + * @param moduleNumber Module number for kinematics. + * @param moduleConfiguration Module constants containing CAN ID's and offsets. + * @param driveFeedforward Drive motor feedforward created by + * {@link SwerveMath#createDriveFeedforward(double, double, double)}. + */ + public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfiguration, + SimpleMotorFeedforward driveFeedforward) + { + // angle = 0; + // speed = 0; + // omega = 0; + // fakePos = 0; + this.moduleNumber = moduleNumber; + configuration = moduleConfiguration; + angleOffset = moduleConfiguration.angleOffset; + + // Initialize Feedforward for drive motor. + feedforward = driveFeedforward; + + // Create motors from configuration and reset them to defaults. + angleMotor = moduleConfiguration.angleMotor; + driveMotor = moduleConfiguration.driveMotor; + angleMotor.factoryDefaults(); + driveMotor.factoryDefaults(); + + // Configure voltage comp, current limit, and ramp rate. + angleMotor.setVoltageCompensation(configuration.physicalCharacteristics.optimalVoltage); + driveMotor.setVoltageCompensation(configuration.physicalCharacteristics.optimalVoltage); + angleMotor.setCurrentLimit(configuration.physicalCharacteristics.angleMotorCurrentLimit); + driveMotor.setCurrentLimit(configuration.physicalCharacteristics.driveMotorCurrentLimit); + angleMotor.setLoopRampRate(configuration.physicalCharacteristics.angleMotorRampRate); + driveMotor.setLoopRampRate(configuration.physicalCharacteristics.driveMotorRampRate); + + // Config angle encoders + absoluteEncoder = moduleConfiguration.absoluteEncoder; + if (absoluteEncoder != null) + { + absoluteEncoder.factoryDefault(); + absoluteEncoder.configure(moduleConfiguration.absoluteEncoderInverted); + angleMotor.setPosition(getAbsolutePosition()); + } + + // Config angle motor/controller + angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle); + angleMotor.configurePIDF(moduleConfiguration.anglePIDF); + angleMotor.configurePIDWrapping(-180, 180); + angleMotor.setInverted(moduleConfiguration.angleMotorInverted); + angleMotor.setMotorBrake(false); + + // Config drive motor/controller + driveMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.drive); + driveMotor.configurePIDF(moduleConfiguration.velocityPIDF); + driveMotor.setInverted(moduleConfiguration.driveMotorInverted); + driveMotor.setMotorBrake(true); + + driveMotor.burnFlash(); + angleMotor.burnFlash(); + + if (SwerveDriveTelemetry.isSimulation) + { + simModule = new SwerveModuleSimulation(); + } + + lastState = getState(); + } + + /** + * Set the voltage compensation for the swerve module motor. + * + * @param optimalVoltage Nominal voltage for operation to output to. + */ + public void setAngleMotorVoltageCompensation(double optimalVoltage) + { + angleMotor.setVoltageCompensation(optimalVoltage); + } + + /** + * Set the voltage compensation for the swerve module motor. + * + * @param optimalVoltage Nominal voltage for operation to output to. + */ + public void setDriveMotorVoltageCompensation(double optimalVoltage) + { + driveMotor.setVoltageCompensation(optimalVoltage); + } + + + /** + * Queue synchronization of the integrated angle encoder with the absolute encoder. + */ + public void queueSynchronizeEncoders() + { + if (absoluteEncoder != null) + { + synchronizeEncoderQueued = true; + } + } + + /** + * Set the desired state of the swerve module.
WARNING: If you are not using one of the functions from + * {@link SwerveDrive} you may screw up {@link SwerveDrive#kinematics} + * + * @param desiredState Desired swerve module state. + * @param isOpenLoop Whether to use open loop (direct percent) or direct velocity control. + * @param force Disables optimizations that prevent movement in the angle motor and forces the desired state + * onto the swerve module. + */ + public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, boolean force) + { + if (moduleStateOptimization) + { + desiredState = SwerveModuleState.optimize(desiredState, + Rotation2d.fromDegrees(getAbsolutePosition())); + } + + if (isOpenLoop) + { + double percentOutput = desiredState.speedMetersPerSecond / maxSpeed; + driveMotor.set(percentOutput); + } else + { + if (desiredState.speedMetersPerSecond != lastState.speedMetersPerSecond) + { + double velocity = desiredState.speedMetersPerSecond; + driveMotor.setReference(velocity, feedforward.calculate(velocity)); + } + } + + // If we are forcing the angle + if (!force) + { + // Prevents module rotation if speed is less than 1% + SwerveMath.antiJitter(desiredState, lastState, Math.min(maxSpeed, 4)); + } + + if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) + { + SmartDashboard.putNumber("Module[" + configuration.name + "] Speed Setpoint:", desiredState.speedMetersPerSecond); + SmartDashboard.putNumber("Module[" + configuration.name + "] Angle Setpoint:", desiredState.angle.getDegrees()); + } + + // Prevent module rotation if angle is the same as the previous angle. + if (desiredState.angle != lastState.angle || synchronizeEncoderQueued) + { + // Synchronize encoders if queued and send in the current position as the value from the absolute encoder. + if (absoluteEncoder != null && synchronizeEncoderQueued) + { + double absoluteEncoderPosition = getAbsolutePosition(); + angleMotor.setPosition(absoluteEncoderPosition); + angleMotor.setReference(desiredState.angle.getDegrees(), 0, absoluteEncoderPosition); + synchronizeEncoderQueued = false; + } else + { + angleMotor.setReference(desiredState.angle.getDegrees(), 0); + } + } + + lastState = desiredState; + + if (SwerveDriveTelemetry.isSimulation) + { + simModule.updateStateAndPosition(desiredState); + } + } + + /** + * Set the angle for the module. + * + * @param angle Angle in degrees. + */ + public void setAngle(double angle) + { + angleMotor.setReference(angle, 0); + lastState.angle = Rotation2d.fromDegrees(angle); + } + + /** + * Get the Swerve Module state. + * + * @return Current SwerveModule state. + */ + public SwerveModuleState getState() + { + double velocity; + Rotation2d azimuth; + if (!SwerveDriveTelemetry.isSimulation) + { + velocity = driveMotor.getVelocity(); + azimuth = Rotation2d.fromDegrees(getAbsolutePosition()); + } else + { + return simModule.getState(); + } + return new SwerveModuleState(velocity, azimuth); + } + + /** + * Get the position of the swerve module. + * + * @return {@link SwerveModulePosition} of the swerve module. + */ + public SwerveModulePosition getPosition() + { + double position; + Rotation2d azimuth; + if (!SwerveDriveTelemetry.isSimulation) + { + position = driveMotor.getPosition(); + azimuth = Rotation2d.fromDegrees(getAbsolutePosition()); + } else + { + return simModule.getPosition(); + } + if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) + { + SmartDashboard.putNumber("Module[" + configuration.name + "] Angle", azimuth.getDegrees()); + } + return new SwerveModulePosition(position, azimuth); + } + + /** + * Get the absolute position. Falls back to relative position on reading failure. + * + * @return Absolute encoder angle in degrees in the range [0, 360). + */ + public double getAbsolutePosition() + { + double angle; + if (absoluteEncoder != null) + { + angle = absoluteEncoder.getAbsolutePosition() - angleOffset; + if (absoluteEncoder.readingError) + { + angle = getRelativePosition(); + } + } else + { + angle = getRelativePosition(); + } + angle %= 360; + if (angle < 0.0) + { + angle += 360; + } + + return angle; + } + + /** + * Get the relative angle in degrees. + * + * @return Angle in degrees. + */ + public double getRelativePosition() + { + return angleMotor.getPosition(); + } + + /** + * Set the brake mode. + * + * @param brake Set the brake mode. + */ + public void setMotorBrake(boolean brake) + { + driveMotor.setMotorBrake(brake); + } + + /** + * Set the conversion factor for the angle/azimuth motor controller. + * + * @param conversionFactor Angle motor conversion factor for PID, should be generated from + * {@link SwerveMath#calculateDegreesPerSteeringRotation(double, double)} or calculated. + */ + public void setAngleMotorConversionFactor(double conversionFactor) + { + angleMotor.configureIntegratedEncoder(conversionFactor); + } + + /** + * Set the conversion factor for the drive motor controller. + * + * @param conversionFactor Drive motor conversion factor for PID, should be generated from + * {@link SwerveMath#calculateMetersPerRotation(double, double, double)} or calculated. + */ + public void setDriveMotorConversionFactor(double conversionFactor) + { + driveMotor.configureIntegratedEncoder(conversionFactor); + } + + /** + * Get the angle {@link SwerveMotor} for the {@link SwerveModule}. + * + * @return {@link SwerveMotor} for the angle/steering motor of the module. + */ + public SwerveMotor getAngleMotor() + { + return angleMotor; + } + + /** + * Get the drive {@link SwerveMotor} for the {@link SwerveModule}. + * + * @return {@link SwerveMotor} for the drive motor of the module. + */ + public SwerveMotor getDriveMotor() + { + return driveMotor; + } + + /** + * Fetch the {@link SwerveModuleConfiguration} for the {@link SwerveModule} with the parsed configurations. + * + * @return {@link SwerveModuleConfiguration} for the {@link SwerveModule}. + */ + public SwerveModuleConfiguration getConfiguration() + { + return configuration; + } + + /** + * Push absolute encoder offset in the memory of the encoder or controller. Also removes the internal angle offset. + */ + public void pushOffsetsToControllers() + { + if (absoluteEncoder != null) + { + if (absoluteEncoder.setAbsoluteEncoderOffset(angleOffset)) + { + angleOffset = 0; + } else + { + DriverStation.reportWarning( + "Pushing the Absolute Encoder offset to the encoder failed on module #" + moduleNumber, false); + } + } else + { + DriverStation.reportWarning("There is no Absolute Encoder on module #" + moduleNumber, false); + } + } + + /** + * Restore internal offset in YAGSL and either sets absolute encoder offset to 0 or restores old value. + */ + public void restoreInternalOffset() + { + absoluteEncoder.setAbsoluteEncoderOffset(0); + angleOffset = configuration.angleOffset; + } + + /** + * Get if the last Absolute Encoder had a read issue, such as it does not exist. + * + * @return If the last Absolute Encoder had a read issue, or absolute encoder does not exist. + */ + public boolean getAbsoluteEncoderReadIssue() + { + if (absoluteEncoder == null) + { + return true; + } else + { + return absoluteEncoder.readingError; + } + } +} diff --git a/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java b/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java new file mode 100644 index 00000000..1b830b1b --- /dev/null +++ b/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java @@ -0,0 +1,120 @@ +package swervelib.encoders; + +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotController; + +/** + * Swerve Absolute Encoder for Thrifty Encoders and other analog encoders. + */ +public class AnalogAbsoluteEncoderSwerve extends SwerveAbsoluteEncoder +{ + // Entire class inspired by 5010 + // Source: https://github.com/FRC5010/FRCLibrary/blob/main/FRC5010Example2023/src/main/java/frc/robot/FRC5010/sensors/AnalogInput5010.java + /** + * Encoder as Analog Input. + */ + public AnalogInput encoder; + /** + * Inversion state of the encoder. + */ + private boolean inverted = false; + + /** + * Construct the Thrifty Encoder as a Swerve Absolute Encoder. + * + * @param encoder Encoder to construct. + */ + public AnalogAbsoluteEncoderSwerve(AnalogInput encoder) + { + this.encoder = encoder; + } + + /** + * Construct the Encoder given the analog input channel. + * + * @param channel Analog Input channel of which the encoder resides. + */ + public AnalogAbsoluteEncoderSwerve(int channel) + { + this(new AnalogInput(channel)); + } + + /** + * Reset the encoder to factory defaults. + */ + @Override + public void factoryDefault() + { + // Do nothing + } + + /** + * Clear sticky faults on the encoder. + */ + @Override + public void clearStickyFaults() + { + // Do nothing + } + + /** + * Configure the absolute encoder to read from [0, 360) per second. + * + * @param inverted Whether the encoder is inverted. + */ + @Override + public void configure(boolean inverted) + { + this.inverted = inverted; + } + + /** + * Get the absolute position of the encoder. + * + * @return Absolute position in degrees from [0, 360). + */ + @Override + public double getAbsolutePosition() + { + return (inverted ? -1.0 : 1.0) * (encoder.getAverageVoltage() / RobotController.getVoltage5V()) * 360; + } + + /** + * Get the instantiated absolute encoder Object. + * + * @return Absolute encoder object. + */ + @Override + public Object getAbsoluteEncoder() + { + return encoder; + } + + /** + * Cannot Set the offset of an Analog Absolute Encoder. + * + * @param offset the offset the Absolute Encoder uses as the zero point. + * @return Will always be false as setting the offset is unsupported of an Analog absolute encoder. + */ + @Override + public boolean setAbsoluteEncoderOffset(double offset) + { + //Do Nothing + DriverStation.reportWarning( + "Cannot Set Absolute Encoder Offset of Analog Encoders Channel #" + encoder.getChannel(), false); + return false; + } + + /** + * Get the velocity in degrees/sec. + * + * @return velocity in degrees/sec. + */ + @Override + public double getVelocity() + { + DriverStation.reportWarning("The Analog Absolute encoder may not report accurate velocities!", true); + return encoder.getValue(); + } +} diff --git a/src/main/java/swervelib/encoders/CANCoderSwerve.java b/src/main/java/swervelib/encoders/CANCoderSwerve.java new file mode 100644 index 00000000..5d8c3283 --- /dev/null +++ b/src/main/java/swervelib/encoders/CANCoderSwerve.java @@ -0,0 +1,171 @@ +package swervelib.encoders; + +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.CANcoderConfigurator; +import com.ctre.phoenix6.configs.MagnetSensorConfigs; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; +import com.ctre.phoenix6.signals.MagnetHealthValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import edu.wpi.first.wpilibj.DriverStation; + +/** + * Swerve Absolute Encoder for CTRE CANCoders. + */ +public class CANCoderSwerve extends SwerveAbsoluteEncoder +{ + + /** + * CANCoder with WPILib sendable and support. + */ + public CANcoder encoder; + + /** + * Initialize the CANCoder on the standard CANBus. + * + * @param id CAN ID. + */ + public CANCoderSwerve(int id) + { + encoder = new CANcoder(id); + } + + /** + * Initialize the CANCoder on the CANivore. + * + * @param id CAN ID. + * @param canbus CAN bus to initialize it on. + */ + public CANCoderSwerve(int id, String canbus) + { + encoder = new CANcoder(id, canbus); + } + + /** + * Reset the encoder to factory defaults. + */ + @Override + public void factoryDefault() + { + encoder.getConfigurator().apply(new CANcoderConfiguration()); + } + + /** + * Clear sticky faults on the encoder. + */ + @Override + public void clearStickyFaults() + { + encoder.clearStickyFaults(); + } + + /** + * Configure the absolute encoder to read from [0, 360) per second. + * + * @param inverted Whether the encoder is inverted. + */ + @Override + public void configure(boolean inverted) + { + CANcoderConfigurator cfg = encoder.getConfigurator(); + MagnetSensorConfigs magnetSensorConfiguration = new MagnetSensorConfigs(); + cfg.refresh(magnetSensorConfiguration); + cfg.apply(magnetSensorConfiguration + .withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1) + .withSensorDirection(inverted ? SensorDirectionValue.Clockwise_Positive + : SensorDirectionValue.CounterClockwise_Positive)); + } + + /** + * Get the absolute position of the encoder. Sets {@link SwerveAbsoluteEncoder#readingError} on erroneous readings. + * + * @return Absolute position in degrees from [0, 360). + */ + @Override + public double getAbsolutePosition() + { + readingError = false; + MagnetHealthValue strength = encoder.getMagnetHealth().getValue(); + + if (strength != MagnetHealthValue.Magnet_Green) + { + DriverStation.reportWarning( + "CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.\n", false); + } + if (strength == MagnetHealthValue.Magnet_Invalid || strength == MagnetHealthValue.Magnet_Red) + { + readingError = true; + DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " reading was faulty.\n", false); + return 0; + } + StatusSignal angle = encoder.getAbsolutePosition().refresh(); + + // Taken from democat's library. + // Source: https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/CanCoderFactoryBuilder.java#L51-L74 + for (int i = 0; i < maximumRetries; i++) + { + if (angle.getStatus() == StatusCode.OK) + { + break; + } + angle = angle.waitForUpdate(0.01); + } + if (angle.getStatus() != StatusCode.OK) + { + readingError = true; + DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " reading was faulty, ignoring.\n", false); + } + + return angle.getValue() * 360; + } + + /** + * Get the instantiated absolute encoder Object. + * + * @return Absolute encoder object. + */ + @Override + public Object getAbsoluteEncoder() + { + return encoder; + } + + /** + * Sets the Absolute Encoder Offset within the CANcoder's Memory. + * + * @param offset the offset the Absolute Encoder uses as the zero point in degrees. + * @return if setting Absolute Encoder Offset was successful or not. + */ + @Override + public boolean setAbsoluteEncoderOffset(double offset) + { + CANcoderConfigurator cfg = encoder.getConfigurator(); + MagnetSensorConfigs magCfg = new MagnetSensorConfigs(); + StatusCode error = cfg.refresh(magCfg); + if (error != StatusCode.OK) + { + return false; + } + error = cfg.apply(magCfg.withMagnetOffset(offset / 360)); + if (error == StatusCode.OK) + { + return true; + } + DriverStation.reportWarning( + "Failure to set CANCoder " + encoder.getDeviceID() + " Absolute Encoder Offset Error: " + error, false); + return false; + } + + /** + * Get the velocity in degrees/sec. + * + * @return velocity in degrees/sec. + */ + @Override + public double getVelocity() + { + return encoder.getVelocity().getValue() * 360; + } +} diff --git a/src/main/java/swervelib/encoders/CanAndCoderSwerve.java b/src/main/java/swervelib/encoders/CanAndCoderSwerve.java new file mode 100644 index 00000000..4b214984 --- /dev/null +++ b/src/main/java/swervelib/encoders/CanAndCoderSwerve.java @@ -0,0 +1,107 @@ +package swervelib.encoders; + +import com.reduxrobotics.sensors.canandcoder.Canandcoder; +import edu.wpi.first.wpilibj.DriverStation; + +/** + * HELIUM {@link Canandcoder} from ReduxRobotics absolute encoder, attached through the CAN bus. + */ +public class CanAndCoderSwerve extends SwerveAbsoluteEncoder +{ + + /** + * The {@link Canandcoder} representing the CANandCoder on the CAN bus. + */ + public Canandcoder encoder; + /** + * Inversion state of the encoder. + */ + private boolean inverted = false; + + /** + * Create the {@link Canandcoder} + * + * @param canid The CAN ID whenever the CANandCoder is operating on the CANBus. + */ + public CanAndCoderSwerve(int canid) + { + encoder = new Canandcoder(canid); + } + + /** + * Reset the encoder to factory defaults. + */ + @Override + public void factoryDefault() + { + encoder.resetFactoryDefaults(false); + } + + /** + * Clear sticky faults on the encoder. + */ + @Override + public void clearStickyFaults() + { + encoder.clearStickyFaults(); + } + + /** + * Configure the CANandCoder to read from [0, 360) per second. + * + * @param inverted Whether the encoder is inverted. + */ + @Override + public void configure(boolean inverted) + { + this.inverted = inverted; + } + + /** + * Get the absolute position of the encoder. + * + * @return Absolute position in degrees from [0, 360). + */ + @Override + public double getAbsolutePosition() + { + return (inverted ? -1.0 : 1.0) * encoder.getPosition() * 360; + } + + /** + * Get the instantiated absolute encoder Object. + * + * @return Absolute encoder object. + */ + @Override + public Object getAbsoluteEncoder() + { + return encoder; + } + + /** + * Cannot set the offset of the CanAndCoder. + * + * @param offset the offset the Absolute Encoder uses as the zero point. + * @return always false due to CanAndCoder not supporting offset changing. + */ + @Override + public boolean setAbsoluteEncoderOffset(double offset) + { + //CanAndCoder does not support Absolute Offset Changing + DriverStation.reportWarning("Cannot Set Absolute Encoder Offset of CanAndCoders ID: " + encoder.getAddress(), + false); + return false; + } + + /** + * Get the velocity in degrees/sec. + * + * @return velocity in degrees/sec. + */ + @Override + public double getVelocity() + { + return encoder.getVelocity(); + } +} diff --git a/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java b/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java new file mode 100644 index 00000000..0361aec8 --- /dev/null +++ b/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java @@ -0,0 +1,112 @@ +package swervelib.encoders; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DutyCycleEncoder; + +/** + * DutyCycle encoders such as "US Digital MA3 with PWM Output, the CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag + * Encoder." attached via a PWM lane. + *

+ * Credits to + * + * p2reneker25 for building this. + */ +public class PWMDutyCycleEncoderSwerve extends SwerveAbsoluteEncoder +{ + + /** + * Duty Cycle Encoder. + */ + private final DutyCycleEncoder encoder; + /** + * Inversion state. + */ + private boolean isInverted; + + /** + * Constructor for the PWM duty cycle encoder. + * + * @param pin PWM lane for the encoder. + */ + public PWMDutyCycleEncoderSwerve(int pin) + { + encoder = new DutyCycleEncoder(pin); + } + + /** + * Configure the inversion state of the encoder. + * + * @param inverted Whether the encoder is inverted. + */ + @Override + public void configure(boolean inverted) + { + isInverted = inverted; + } + + /** + * Get the absolute position of the encoder. + * + * @return Absolute position in degrees from [0, 360). + */ + @Override + public double getAbsolutePosition() + { + return (isInverted ? -1.0 : 1.0) * encoder.getAbsolutePosition() * 360; + } + + /** + * Get the encoder object. + * + * @return {@link DutyCycleEncoder} from the class. + */ + @Override + public Object getAbsoluteEncoder() + { + return encoder; + } + + /** + * Get the velocity in degrees/sec. + * + * @return velocity in degrees/sec. + */ + @Override + public double getVelocity() + { + DriverStation.reportWarning("The PWM Duty Cycle encoder may not report accurate velocities!", true); + return encoder.get(); + } + + /** + * Reset the encoder to factory defaults. + */ + @Override + public void factoryDefault() + { + // Do nothing + } + + /** + * Clear sticky faults on the encoder. + */ + @Override + public void clearStickyFaults() + { + // Do nothing + } + + /** + * Sets the offset of the Encoder in the WPILib Encoder Library. + * + * @param offset the offset the Absolute Encoder uses as the zero point. + * @return Always true due to no external device commands. + */ + @Override + public boolean setAbsoluteEncoderOffset(double offset) + { + encoder.setPositionOffset(offset); + + return true; + } +} \ No newline at end of file diff --git a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java new file mode 100644 index 00000000..1f79cbf4 --- /dev/null +++ b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java @@ -0,0 +1,130 @@ +package swervelib.encoders; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.REVLibError; +import com.revrobotics.SparkMaxAnalogSensor; +import com.revrobotics.SparkMaxAnalogSensor.Mode; +import edu.wpi.first.wpilibj.DriverStation; +import java.util.function.Supplier; +import swervelib.motors.SwerveMotor; + +/** + * SparkMax absolute encoder, attached through the data port analog pin. + */ +public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder +{ + + /** + * The {@link SparkMaxAnalogSensor} representing the duty cycle encoder attached to the SparkMax analog port. + */ + public SparkMaxAnalogSensor encoder; + + /** + * Create the {@link SparkMaxAnalogEncoderSwerve} object as a analog sensor from the {@link CANSparkMax} motor data + * port analog pin. + * + * @param motor Motor to create the encoder from. + */ + public SparkMaxAnalogEncoderSwerve(SwerveMotor motor) + { + if (motor.getMotor() instanceof CANSparkMax) + { + encoder = ((CANSparkMax) motor.getMotor()).getAnalog(Mode.kAbsolute); + } else + { + throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax"); + } + } + + /** + * Run the configuration until it succeeds or times out. + * + * @param config Lambda supplier returning the error state. + */ + private void configureSparkMax(Supplier config) + { + for (int i = 0; i < maximumRetries; i++) + { + if (config.get() == REVLibError.kOk) + { + return; + } + } + DriverStation.reportWarning("Failure configuring encoder", true); + } + + /** + * Reset the encoder to factory defaults. + */ + @Override + public void factoryDefault() + { + // Do nothing + } + + /** + * Clear sticky faults on the encoder. + */ + @Override + public void clearStickyFaults() + { + // Do nothing + } + + /** + * Configure the absolute encoder to read from [0, 360) per second. + * + * @param inverted Whether the encoder is inverted. + */ + @Override + public void configure(boolean inverted) + { + encoder.setInverted(inverted); + } + + /** + * Get the absolute position of the encoder. + * + * @return Absolute position in degrees from [0, 360). + */ + @Override + public double getAbsolutePosition() + { + return encoder.getPosition(); + } + + /** + * Get the instantiated absolute encoder Object. + * + * @return Absolute encoder object. + */ + @Override + public Object getAbsoluteEncoder() + { + return encoder; + } + + /** + * Sets the Absolute Encoder offset at the Encoder Level. + * + * @param offset the offset the Absolute Encoder uses as the zero point. + * @return if setting Absolute Encoder Offset was successful or not. + */ + @Override + public boolean setAbsoluteEncoderOffset(double offset) + { + DriverStation.reportWarning("SparkMax Analog Sensor's do not support integrated offsets", true); + return false; + } + + /** + * Get the velocity in degrees/sec. + * + * @return velocity in degrees/sec. + */ + @Override + public double getVelocity() + { + return encoder.getVelocity(); + } +} diff --git a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java new file mode 100644 index 00000000..87f97822 --- /dev/null +++ b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java @@ -0,0 +1,141 @@ +package swervelib.encoders; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.CANSparkMax; +import com.revrobotics.REVLibError; +import com.revrobotics.SparkMaxAbsoluteEncoder.Type; +import edu.wpi.first.wpilibj.DriverStation; +import java.util.function.Supplier; +import swervelib.motors.SwerveMotor; + +/** + * SparkMax absolute encoder, attached through the data port. + */ +public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder +{ + + /** + * The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkMax. + */ + public AbsoluteEncoder encoder; + + /** + * Create the {@link SparkMaxEncoderSwerve} object as a duty cycle from the {@link CANSparkMax} motor. + * + * @param motor Motor to create the encoder from. + * @param conversionFactor The conversion factor to set if the output is not from 0 to 360. + */ + public SparkMaxEncoderSwerve(SwerveMotor motor, int conversionFactor) + { + if (motor.getMotor() instanceof CANSparkMax) + { + encoder = ((CANSparkMax) motor.getMotor()).getAbsoluteEncoder(Type.kDutyCycle); + configureSparkMax(() -> encoder.setVelocityConversionFactor(conversionFactor)); + configureSparkMax(() -> encoder.setPositionConversionFactor(conversionFactor)); + } else + { + throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax"); + } + } + + /** + * Run the configuration until it succeeds or times out. + * + * @param config Lambda supplier returning the error state. + */ + private void configureSparkMax(Supplier config) + { + for (int i = 0; i < maximumRetries; i++) + { + if (config.get() == REVLibError.kOk) + { + return; + } + } + DriverStation.reportWarning("Failure configuring encoder", true); + } + + /** + * Reset the encoder to factory defaults. + */ + @Override + public void factoryDefault() + { + // Do nothing + } + + /** + * Clear sticky faults on the encoder. + */ + @Override + public void clearStickyFaults() + { + // Do nothing + } + + /** + * Configure the absolute encoder to read from [0, 360) per second. + * + * @param inverted Whether the encoder is inverted. + */ + @Override + public void configure(boolean inverted) + { + encoder.setInverted(inverted); + } + + /** + * Get the absolute position of the encoder. + * + * @return Absolute position in degrees from [0, 360). + */ + @Override + public double getAbsolutePosition() + { + return encoder.getPosition(); + } + + /** + * Get the instantiated absolute encoder Object. + * + * @return Absolute encoder object. + */ + @Override + public Object getAbsoluteEncoder() + { + return encoder; + } + + /** + * Sets the Absolute Encoder Offset inside of the SparkMax's Memory. + * + * @param offset the offset the Absolute Encoder uses as the zero point. + * @return if setting Absolute Encoder Offset was successful or not. + */ + @Override + public boolean setAbsoluteEncoderOffset(double offset) + { + REVLibError error = null; + for (int i = 0; i < maximumRetries; i++) + { + error = encoder.setZeroOffset(offset); + if (error == REVLibError.kOk) + { + return true; + } + } + DriverStation.reportWarning("Failure to set Absolute Encoder Offset Error: " + error, false); + return false; + } + + /** + * Get the velocity in degrees/sec. + * + * @return velocity in degrees/sec. + */ + @Override + public double getVelocity() + { + return encoder.getVelocity(); + } +} diff --git a/src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java b/src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java new file mode 100644 index 00000000..a01e4c47 --- /dev/null +++ b/src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java @@ -0,0 +1,62 @@ +package swervelib.encoders; + +/** + * Swerve abstraction class to define a standard interface with absolute encoders for swerve modules.. + */ +public abstract class SwerveAbsoluteEncoder +{ + + /** + * The maximum amount of times the swerve encoder will attempt to configure itself if failures occur. + */ + public final int maximumRetries = 5; + /** + * Last angle reading was faulty. + */ + public boolean readingError = false; + + /** + * Reset the encoder to factory defaults. + */ + public abstract void factoryDefault(); + + /** + * Clear sticky faults on the encoder. + */ + public abstract void clearStickyFaults(); + + /** + * Configure the absolute encoder to read from [0, 360) per second. + * + * @param inverted Whether the encoder is inverted. + */ + public abstract void configure(boolean inverted); + + /** + * Get the absolute position of the encoder. + * + * @return Absolute position in degrees from [0, 360). + */ + public abstract double getAbsolutePosition(); + + /** + * Get the instantiated absolute encoder Object. + * + * @return Absolute encoder object. + */ + public abstract Object getAbsoluteEncoder(); + + /** + * Sets the Absolute Encoder offset at the Encoder Level. + * + * @param offset the offset the Absolute Encoder uses as the zero point. + * @return if setting Absolute Encoder Offset was successful or not. + */ + public abstract boolean setAbsoluteEncoderOffset(double offset); + + /** + * Get the velocity in degrees/sec. + * @return velocity in degrees/sec. + */ + public abstract double getVelocity(); +} diff --git a/src/main/java/swervelib/encoders/package-info.java b/src/main/java/swervelib/encoders/package-info.java new file mode 100644 index 00000000..ab968083 --- /dev/null +++ b/src/main/java/swervelib/encoders/package-info.java @@ -0,0 +1,4 @@ +/** + * Absolute encoders for the swerve drive, all implement {@link swervelib.encoders.SwerveAbsoluteEncoder}. + */ +package swervelib.encoders; diff --git a/src/main/java/swervelib/imu/ADIS16448Swerve.java b/src/main/java/swervelib/imu/ADIS16448Swerve.java new file mode 100644 index 00000000..d60f8c6a --- /dev/null +++ b/src/main/java/swervelib/imu/ADIS16448Swerve.java @@ -0,0 +1,108 @@ +package swervelib.imu; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.wpilibj.ADIS16448_IMU; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.util.Optional; + +/** + * IMU Swerve class for the {@link ADIS16448_IMU} device. + */ +public class ADIS16448Swerve extends SwerveIMU +{ + + /** + * {@link ADIS16448_IMU} device to read the current headings from. + */ + private final ADIS16448_IMU imu; + /** + * Offset for the ADIS16448. + */ + private Rotation3d offset = new Rotation3d(); + + /** + * Construct the ADIS16448 imu and reset default configurations. Publish the gyro to the SmartDashboard. + */ + public ADIS16448Swerve() + { + imu = new ADIS16448_IMU(); + factoryDefault(); + SmartDashboard.putData(imu); + } + + /** + * Reset IMU to factory default. + */ + @Override + public void factoryDefault() + { + offset = new Rotation3d(0, 0, 0); + imu.calibrate(); + } + + /** + * Clear sticky faults on IMU. + */ + @Override + public void clearStickyFaults() + { + // Do nothing. + } + + /** + * Set the gyro offset. + * + * @param offset gyro offset as a {@link Rotation3d}. + */ + public void setOffset(Rotation3d offset) + { + this.offset = offset; + } + + /** + * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + public Rotation3d getRawRotation3d() + { + return new Rotation3d(Math.toRadians(-imu.getGyroAngleX()), + Math.toRadians(-imu.getGyroAngleY()), + Math.toRadians(-imu.getGyroAngleZ())); + } + + /** + * Fetch the {@link Rotation3d} from the IMU. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + @Override + public Rotation3d getRotation3d() + { + return getRawRotation3d().minus(offset); + } + + /** + * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns + * empty. + * + * @return {@link Translation3d} of the acceleration. + */ + @Override + public Optional getAccel() + { + return Optional.of(new Translation3d(imu.getAccelX(), imu.getAccelY(), imu.getAccelZ())); + } + + /** + * Get the instantiated IMU object. + * + * @return IMU object. + */ + @Override + public Object getIMU() + { + return imu; + } +} diff --git a/src/main/java/swervelib/imu/ADIS16470Swerve.java b/src/main/java/swervelib/imu/ADIS16470Swerve.java new file mode 100644 index 00000000..c93dcb67 --- /dev/null +++ b/src/main/java/swervelib/imu/ADIS16470Swerve.java @@ -0,0 +1,107 @@ +package swervelib.imu; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.wpilibj.ADIS16470_IMU; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.util.Optional; + +/** + * IMU Swerve class for the {@link ADIS16470_IMU} device. + */ +public class ADIS16470Swerve extends SwerveIMU +{ + + /** + * {@link ADIS16470_IMU} device to read the current headings from. + */ + private final ADIS16470_IMU imu; + /** + * Offset for the ADIS16470. + */ + private Rotation3d offset = new Rotation3d(); + + /** + * Construct the ADIS16470 imu and reset default configurations. Publish the gyro to the SmartDashboard. + */ + public ADIS16470Swerve() + { + imu = new ADIS16470_IMU(); + offset = new Rotation3d(); + factoryDefault(); + SmartDashboard.putData(imu); + } + + /** + * Reset IMU to factory default. + */ + @Override + public void factoryDefault() + { + offset = new Rotation3d(0, 0, 0); + imu.calibrate(); + } + + /** + * Clear sticky faults on IMU. + */ + @Override + public void clearStickyFaults() + { + // Do nothing. + } + + /** + * Set the gyro offset. + * + * @param offset gyro offset as a {@link Rotation3d}. + */ + public void setOffset(Rotation3d offset) + { + this.offset = offset; + } + + /** + * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + public Rotation3d getRawRotation3d() + { + return new Rotation3d(0, 0, Math.toRadians(-imu.getAngle())); + } + + /** + * Fetch the {@link Rotation3d} from the IMU. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + @Override + public Rotation3d getRotation3d() + { + return getRawRotation3d().minus(offset); + } + + /** + * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns + * empty. + * + * @return {@link Translation3d} of the acceleration as an {@link Optional}. + */ + @Override + public Optional getAccel() + { + return Optional.of(new Translation3d(imu.getAccelX(), imu.getAccelY(), imu.getAccelZ())); + } + + /** + * Get the instantiated IMU object. + * + * @return IMU object. + */ + @Override + public Object getIMU() + { + return imu; + } +} diff --git a/src/main/java/swervelib/imu/ADXRS450Swerve.java b/src/main/java/swervelib/imu/ADXRS450Swerve.java new file mode 100644 index 00000000..f313fb0d --- /dev/null +++ b/src/main/java/swervelib/imu/ADXRS450Swerve.java @@ -0,0 +1,106 @@ +package swervelib.imu; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.wpilibj.ADXRS450_Gyro; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.util.Optional; + +/** + * IMU Swerve class for the {@link ADXRS450_Gyro} device. + */ +public class ADXRS450Swerve extends SwerveIMU +{ + + /** + * {@link ADXRS450_Gyro} device to read the current headings from. + */ + private final ADXRS450_Gyro imu; + /** + * Offset for the ADXRS450. + */ + private Rotation3d offset = new Rotation3d(); + + /** + * Construct the ADXRS450 imu and reset default configurations. Publish the gyro to the SmartDashboard. + */ + public ADXRS450Swerve() + { + imu = new ADXRS450_Gyro(); + factoryDefault(); + SmartDashboard.putData(imu); + } + + /** + * Reset IMU to factory default. + */ + @Override + public void factoryDefault() + { + imu.calibrate(); + offset = new Rotation3d(0, 0, 0);//Math.toRadians(-imu.getAngle())); + } + + /** + * Clear sticky faults on IMU. + */ + @Override + public void clearStickyFaults() + { + // Do nothing. + } + + /** + * Set the gyro offset. + * + * @param offset gyro offset as a {@link Rotation3d}. + */ + public void setOffset(Rotation3d offset) + { + this.offset = offset; + } + + /** + * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + public Rotation3d getRawRotation3d() + { + return new Rotation3d(0, 0, Math.toRadians(-imu.getAngle())); + } + + /** + * Fetch the {@link Rotation3d} from the IMU. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + @Override + public Rotation3d getRotation3d() + { + return getRawRotation3d().minus(offset); + } + + /** + * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns + * empty. + * + * @return {@link Translation3d} of the acceleration as an {@link Optional}. + */ + @Override + public Optional getAccel() + { + return Optional.empty(); + } + + /** + * Get the instantiated IMU object. + * + * @return IMU object. + */ + @Override + public Object getIMU() + { + return imu; + } +} diff --git a/src/main/java/swervelib/imu/AnalogGyroSwerve.java b/src/main/java/swervelib/imu/AnalogGyroSwerve.java new file mode 100644 index 00000000..590666f7 --- /dev/null +++ b/src/main/java/swervelib/imu/AnalogGyroSwerve.java @@ -0,0 +1,113 @@ +package swervelib.imu; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.wpilibj.AnalogGyro; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.util.Optional; + +/** + * Creates a IMU for {@link edu.wpi.first.wpilibj.AnalogGyro} devices, only uses yaw. + */ +public class AnalogGyroSwerve extends SwerveIMU +{ + + /** + * Gyroscope object. + */ + private final AnalogGyro gyro; + /** + * Offset for the analog gyro. + */ + private Rotation3d offset = new Rotation3d(); + + /** + * Analog port in which the gyroscope is connected. Can only be attached to analog ports 0 or 1. + * + * @param channel Analog port 0 or 1. + */ + public AnalogGyroSwerve(int channel) + { + if (!(channel == 0 || channel == 1)) + { + throw new RuntimeException( + "Analog Gyroscope must be attached to port 0 or 1 on the roboRIO.\n"); + } + gyro = new AnalogGyro(channel); + factoryDefault(); + SmartDashboard.putData(gyro); + } + + /** + * Reset IMU to factory default. + */ + @Override + public void factoryDefault() + { + gyro.calibrate(); + offset = new Rotation3d(0, 0, 0); + } + + /** + * Clear sticky faults on IMU. + */ + @Override + public void clearStickyFaults() + { + // Do nothing. + } + + /** + * Set the gyro offset. + * + * @param offset gyro offset as a {@link Rotation3d}. + */ + public void setOffset(Rotation3d offset) + { + this.offset = offset; + } + + /** + * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + public Rotation3d getRawRotation3d() + { + return new Rotation3d(0, 0, Math.toRadians(-gyro.getAngle())); + } + + /** + * Fetch the {@link Rotation3d} from the IMU. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + @Override + public Rotation3d getRotation3d() + { + return getRawRotation3d().minus(offset); + } + + /** + * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns + * empty. + * + * @return {@link Translation3d} of the acceleration as an {@link Optional}. + */ + @Override + public Optional getAccel() + { + return Optional.empty(); + } + + /** + * Get the instantiated IMU object. + * + * @return IMU object. + */ + @Override + public Object getIMU() + { + return gyro; + } +} diff --git a/src/main/java/swervelib/imu/NavXSwerve.java b/src/main/java/swervelib/imu/NavXSwerve.java new file mode 100644 index 00000000..f793300b --- /dev/null +++ b/src/main/java/swervelib/imu/NavXSwerve.java @@ -0,0 +1,174 @@ +package swervelib.imu; + +import com.kauailabs.navx.frc.AHRS; +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.I2C; +import edu.wpi.first.wpilibj.SPI; +import edu.wpi.first.wpilibj.SerialPort; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.util.Optional; + +/** + * Communicates with the NavX as the IMU. + */ +public class NavXSwerve extends SwerveIMU +{ + + /** + * NavX IMU. + */ + private AHRS gyro; + /** + * Offset for the NavX. + */ + private Rotation3d offset = new Rotation3d(); + + /** + * Constructor for the NavX swerve. + * + * @param port Serial Port to connect to. + */ + public NavXSwerve(SerialPort.Port port) + { + try + { + /* Communicate w/navX-MXP via the MXP SPI Bus. */ + /* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */ + /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */ + gyro = new AHRS(port); + factoryDefault(); + SmartDashboard.putData(gyro); + } catch (RuntimeException ex) + { + DriverStation.reportError("Error instantiating navX: " + ex.getMessage(), true); + } + } + + /** + * Constructor for the NavX swerve. + * + * @param port SPI Port to connect to. + */ + public NavXSwerve(SPI.Port port) + { + try + { + /* Communicate w/navX-MXP via the MXP SPI Bus. */ + /* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */ + /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */ + gyro = new AHRS(port); + factoryDefault(); + SmartDashboard.putData(gyro); + } catch (RuntimeException ex) + { + DriverStation.reportError("Error instantiating navX: " + ex.getMessage(), true); + } + } + + /** + * Constructor for the NavX swerve. + * + * @param port I2C Port to connect to. + */ + public NavXSwerve(I2C.Port port) + { + try + { + /* Communicate w/navX-MXP via the MXP SPI Bus. */ + /* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */ + /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */ + gyro = new AHRS(port); + factoryDefault(); + SmartDashboard.putData(gyro); + } catch (RuntimeException ex) + { + DriverStation.reportError("Error instantiating navX: " + ex.getMessage(), true); + } + } + + /** + * Reset IMU to factory default. + */ + @Override + public void factoryDefault() + { + // gyro.reset(); // Reported to be slow + offset = new Rotation3d(new Quaternion(gyro.getQuaternionW(), + gyro.getQuaternionX(), + gyro.getQuaternionY(), + gyro.getQuaternionZ())); + } + + /** + * Clear sticky faults on IMU. + */ + @Override + public void clearStickyFaults() + { + } + + /** + * Set the gyro offset. + * + * @param offset gyro offset as a {@link Rotation3d}. + */ + public void setOffset(Rotation3d offset) + { + this.offset = offset; + } + + /** + * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + public Rotation3d getRawRotation3d() + { + return new Rotation3d(new Quaternion(gyro.getQuaternionW(), + gyro.getQuaternionX(), + gyro.getQuaternionY(), + gyro.getQuaternionZ())); + } + + /** + * Fetch the {@link Rotation3d} from the IMU. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + @Override + public Rotation3d getRotation3d() + { + return getRawRotation3d().minus(offset); + } + + /** + * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns + * empty. + * + * @return {@link Translation3d} of the acceleration as an {@link Optional}. + */ + @Override + public Optional getAccel() + { + return Optional.of( + new Translation3d( + gyro.getWorldLinearAccelX(), + gyro.getWorldLinearAccelY(), + gyro.getWorldLinearAccelZ()) + .times(9.81)); + } + + /** + * Get the instantiated IMU object. + * + * @return IMU object. + */ + @Override + public Object getIMU() + { + return gyro; + } +} diff --git a/src/main/java/swervelib/imu/Pigeon2Swerve.java b/src/main/java/swervelib/imu/Pigeon2Swerve.java new file mode 100644 index 00000000..61fb6f83 --- /dev/null +++ b/src/main/java/swervelib/imu/Pigeon2Swerve.java @@ -0,0 +1,136 @@ +package swervelib.imu; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.configs.Pigeon2Configurator; +import com.ctre.phoenix6.hardware.Pigeon2; +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.util.Optional; + +/** + * SwerveIMU interface for the Pigeon2 + */ +public class Pigeon2Swerve extends SwerveIMU +{ + + /** + * Pigeon2 IMU device. + */ + Pigeon2 imu; + /** + * Offset for the Pigeon 2. + */ + private Rotation3d offset = new Rotation3d(); + + /** + * Generate the SwerveIMU for pigeon. + * + * @param canid CAN ID for the pigeon + * @param canbus CAN Bus name the pigeon resides on. + */ + public Pigeon2Swerve(int canid, String canbus) + { + imu = new Pigeon2(canid, canbus); + SmartDashboard.putData(imu); + } + + /** + * Generate the SwerveIMU for pigeon. + * + * @param canid CAN ID for the pigeon + */ + public Pigeon2Swerve(int canid) + { + this(canid, ""); + } + + /** + * Reset IMU to factory default. + */ + @Override + public void factoryDefault() + { + Pigeon2Configurator cfg = imu.getConfigurator(); + Pigeon2Configuration config = new Pigeon2Configuration(); + + // Compass utilization causes readings to jump dramatically in some cases. + cfg.apply(config.Pigeon2Features.withEnableCompass(false)); + } + + /** + * Clear sticky faults on IMU. + */ + @Override + public void clearStickyFaults() + { + imu.clearStickyFaults(); + } + + /** + * Set the gyro offset. + * + * @param offset gyro offset as a {@link Rotation3d}. + */ + public void setOffset(Rotation3d offset) + { + this.offset = offset; + } + + /** + * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + @Override + public Rotation3d getRawRotation3d() + { + // TODO: Switch to suppliers. + StatusSignal w = imu.getQuatW(); + StatusSignal x = imu.getQuatX(); + StatusSignal y = imu.getQuatY(); + StatusSignal z = imu.getQuatZ(); + return new Rotation3d(new Quaternion(w.refresh().getValue(), x.refresh().getValue(), y.refresh().getValue(), z.refresh().getValue())); + } + + /** + * Fetch the {@link Rotation3d} from the IMU. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + @Override + public Rotation3d getRotation3d() + { + return getRawRotation3d().minus(offset); + } + + /** + * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns + * empty. + * + * @return {@link Translation3d} of the acceleration as an {@link Optional}. + */ + @Override + public Optional getAccel() + { + // TODO: Switch to suppliers. + StatusSignal xAcc = imu.getAccelerationX(); + StatusSignal yAcc = imu.getAccelerationX(); + StatusSignal zAcc = imu.getAccelerationX(); + + return Optional.of(new Translation3d(xAcc.refresh().getValue(), yAcc.refresh().getValue(), zAcc.refresh().getValue()).times(9.81 / 16384.0)); + } + + /** + * Get the instantiated IMU object. + * + * @return IMU object. + */ + @Override + public Object getIMU() + { + return imu; + } +} diff --git a/src/main/java/swervelib/imu/PigeonSwerve.java b/src/main/java/swervelib/imu/PigeonSwerve.java new file mode 100644 index 00000000..44fafcc7 --- /dev/null +++ b/src/main/java/swervelib/imu/PigeonSwerve.java @@ -0,0 +1,113 @@ +package swervelib.imu; + +import com.ctre.phoenix.sensors.WPI_PigeonIMU; +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.util.Optional; + +/** + * SwerveIMU interface for the Pigeon. + */ +public class PigeonSwerve extends SwerveIMU +{ + + /** + * Pigeon v1 IMU device. + */ + WPI_PigeonIMU imu; + /** + * Offset for the Pigeon. + */ + private Rotation3d offset = new Rotation3d(); + + /** + * Generate the SwerveIMU for pigeon. + * + * @param canid CAN ID for the pigeon, does not support CANBus. + */ + public PigeonSwerve(int canid) + { + imu = new WPI_PigeonIMU(canid); + offset = new Rotation3d(); + SmartDashboard.putData(imu); + } + + /** + * Reset IMU to factory default. + */ + @Override + public void factoryDefault() + { + imu.configFactoryDefault(); + } + + /** + * Clear sticky faults on IMU. + */ + @Override + public void clearStickyFaults() + { + imu.clearStickyFaults(); + } + + /** + * Set the gyro offset. + * + * @param offset gyro offset as a {@link Rotation3d}. + */ + public void setOffset(Rotation3d offset) + { + this.offset = offset; + } + + /** + * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + @Override + public Rotation3d getRawRotation3d() + { + double[] wxyz = new double[4]; + imu.get6dQuaternion(wxyz); + return new Rotation3d(new Quaternion(wxyz[0], wxyz[1], wxyz[2], wxyz[3])); + } + + /** + * Fetch the {@link Rotation3d} from the IMU. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + @Override + public Rotation3d getRotation3d() + { + return getRawRotation3d().minus(offset); + } + + /** + * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns + * empty. + * + * @return {@link Translation3d} of the acceleration as an {@link Optional}. + */ + @Override + public Optional getAccel() + { + short[] initial = new short[3]; + imu.getBiasedAccelerometer(initial); + return Optional.of(new Translation3d(initial[0], initial[1], initial[2]).times(9.81 / 16384.0)); + } + + /** + * Get the instantiated IMU object. + * + * @return IMU object. + */ + @Override + public Object getIMU() + { + return imu; + } +} diff --git a/src/main/java/swervelib/imu/SwerveIMU.java b/src/main/java/swervelib/imu/SwerveIMU.java new file mode 100644 index 00000000..11b39daa --- /dev/null +++ b/src/main/java/swervelib/imu/SwerveIMU.java @@ -0,0 +1,58 @@ +package swervelib.imu; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import java.util.Optional; + +/** + * Swerve IMU abstraction to define a standard interface with a swerve drive. + */ +public abstract class SwerveIMU +{ + + /** + * Reset IMU to factory default. + */ + public abstract void factoryDefault(); + + /** + * Clear sticky faults on IMU. + */ + public abstract void clearStickyFaults(); + + /** + * Set the gyro offset. + * + * @param offset gyro offset as a {@link Rotation3d}. + */ + public abstract void setOffset(Rotation3d offset); + + /** + * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + public abstract Rotation3d getRawRotation3d(); + + /** + * Fetch the {@link Rotation3d} from the IMU. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + public abstract Rotation3d getRotation3d(); + + /** + * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns + * empty. + * + * @return {@link Translation3d} of the acceleration as an {@link Optional}. + */ + public abstract Optional getAccel(); + + /** + * Get the instantiated IMU object. + * + * @return IMU object. + */ + public abstract Object getIMU(); +} diff --git a/src/main/java/swervelib/imu/package-info.java b/src/main/java/swervelib/imu/package-info.java new file mode 100644 index 00000000..2b02abc0 --- /dev/null +++ b/src/main/java/swervelib/imu/package-info.java @@ -0,0 +1,4 @@ +/** + * IMUs used for controlling the robot heading. All implement {@link swervelib.imu.SwerveIMU}. + */ +package swervelib.imu; diff --git a/src/main/java/swervelib/math/Matter.java b/src/main/java/swervelib/math/Matter.java new file mode 100644 index 00000000..94329b32 --- /dev/null +++ b/src/main/java/swervelib/math/Matter.java @@ -0,0 +1,41 @@ +package swervelib.math; + +import edu.wpi.first.math.geometry.Translation3d; + +/** + * Object with significant mass that needs to be taken into account. + */ +public class Matter +{ + + /** + * Position in meters from robot center in 3d space. + */ + public Translation3d position; + /** + * Mass in kg of object. + */ + public double mass; + + /** + * Construct an object representing some significant matter on the robot. + * + * @param position Position of the matter in meters. + * @param mass Mass in kg. + */ + public Matter(Translation3d position, double mass) + { + this.mass = mass; + this.position = position; + } + + /** + * Get the center mass of the object. + * + * @return center mass = position * mass + */ + public Translation3d massMoment() + { + return position.times(mass); + } +} diff --git a/src/main/java/swervelib/math/SwerveMath.java b/src/main/java/swervelib/math/SwerveMath.java new file mode 100644 index 00000000..4ab9a2de --- /dev/null +++ b/src/main/java/swervelib/math/SwerveMath.java @@ -0,0 +1,402 @@ +package swervelib.math; + +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.util.List; +import swervelib.SwerveController; +import swervelib.SwerveModule; +import swervelib.parser.SwerveDriveConfiguration; +import swervelib.parser.SwerveModuleConfiguration; +import swervelib.telemetry.SwerveDriveTelemetry; +import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; + +/** + * Mathematical functions which pertain to swerve drive. + */ +public class SwerveMath +{ + + /** + * Calculate the meters per rotation for the integrated encoder. Calculation: (PI * WHEEL DIAMETER IN METERS) / (GEAR + * RATIO * ENCODER RESOLUTION) + * + * @param wheelDiameter Wheel diameter in meters. + * @param driveGearRatio The gear ratio of the drive motor. + * @param pulsePerRotation The number of encoder pulses per rotation. 1 if using an integrated encoder. + * @return Meters per rotation for the drive motor. + */ + public static double calculateMetersPerRotation( + double wheelDiameter, double driveGearRatio, double pulsePerRotation) + { + return (Math.PI * wheelDiameter) / (driveGearRatio * pulsePerRotation); + } + + /** + * Normalize an angle to be within 0 to 360. + * + * @param angle Angle in degrees. + * @return Normalized angle in degrees. + */ + public static double normalizeAngle(double angle) + { + Rotation2d angleRotation = Rotation2d.fromDegrees(angle); + return new Rotation2d(angleRotation.getCos(), angleRotation.getSin()).getDegrees(); + } + + /** + * Algebraically apply a deadband using a piece wise function. + * + * @param value value to apply deadband to. + * @param scaled Use algebra to determine deadband by starting the value at 0 past deadband. + * @param deadband The deadbnad to apply. + * @return Value with deadband applied. + */ + public static double applyDeadband(double value, boolean scaled, double deadband) + { + value = Math.abs(value) > deadband ? value : 0; + return scaled + ? ((1 / (1 - deadband)) * (Math.abs(value) - deadband)) * Math.signum(value) + : value; + } + + /** + * Create the drive feedforward for swerve modules. + * + * @param optimalVoltage Optimal voltage to calculate kV (voltage/max Velocity) + * @param maxSpeed Maximum velocity in meters per second to use for the feed forward, should be + * as close to physical max as possible. + * @param wheelGripCoefficientOfFriction Wheel grip coefficient of friction for kA (voltage/(cof*9.81)) + * @return Drive feedforward for drive motor on a swerve module. + */ + public static SimpleMotorFeedforward createDriveFeedforward(double optimalVoltage, double maxSpeed, + double wheelGripCoefficientOfFriction) + { + double kv = optimalVoltage / maxSpeed; + /// ^ Volt-seconds per meter (max voltage divided by max speed) + double ka = + optimalVoltage + / calculateMaxAcceleration(wheelGripCoefficientOfFriction); + /// ^ Volt-seconds^2 per meter (max voltage divided by max accel) + return new SimpleMotorFeedforward(0, kv, ka); + } + + /** + * Calculate the degrees per steering rotation for the integrated encoder. Encoder conversion values. Drive converts + * motor rotations to linear wheel distance and steering converts motor rotations to module azimuth. + * + * @param angleGearRatio The gear ratio of the steering motor. + * @param pulsePerRotation The number of pulses in a complete rotation for the encoder, 1 if integrated. + * @return Degrees per steering rotation for the angle motor. + */ + public static double calculateDegreesPerSteeringRotation( + double angleGearRatio, double pulsePerRotation) + { + return 360 / (angleGearRatio * pulsePerRotation); + } + + /** + * Calculate the maximum angular velocity. + * + * @param maxSpeed Max speed of the robot in meters per second. + * @param furthestModuleX X of the furthest module in meters. + * @param furthestModuleY Y of the furthest module in meters. + * @return Maximum angular velocity in rad/s. + */ + public static double calculateMaxAngularVelocity( + double maxSpeed, double furthestModuleX, double furthestModuleY) + { + return maxSpeed / new Rotation2d(furthestModuleX, furthestModuleY).getRadians(); + } + + /** + * Calculate the practical maximum acceleration of the robot using the wheel coefficient of friction. + * + * @param cof Coefficient of Friction of the wheel grip tape. + * @return Practical maximum acceleration in m/s/s. + */ + public static double calculateMaxAcceleration(double cof) + { + return cof * 9.81; + } + + /** + * Calculate the maximum theoretical acceleration without friction. + * + * @param stallTorqueNm Stall torque of driving motor in nM. + * @param gearRatio Gear ratio for driving motor number of motor rotations until one wheel rotation. + * @param moduleCount Number of swerve modules. + * @param wheelDiameter Wheel diameter in meters. + * @param robotMass Mass of the robot in kg. + * @return Theoretical maximum acceleration in m/s/s. + */ + public static double calculateMaxAcceleration( + double stallTorqueNm, + double gearRatio, + double moduleCount, + double wheelDiameter, + double robotMass) + { + return (stallTorqueNm * gearRatio * moduleCount) / ((wheelDiameter / 2) * robotMass); + } + + /** + * Calculates the maximum acceleration allowed in a direction without tipping the robot. Reads arm position from + * NetworkTables and is passed the direction in question. + * + * @param angle The direction in which to calculate max acceleration, as a Rotation2d. Note that this is + * robot-relative. + * @param matter Matter that the robot is composed of in kg. (Includes chassis) + * @param robotMass The weight of the robot in kg. (Including manipulators, etc). + * @param config The swerve drive configuration. + * @return Maximum acceleration allowed in the robot direction. + */ + private static double calcMaxAccel( + Rotation2d angle, + List matter, + double robotMass, + SwerveDriveConfiguration config) + { + // Calculate the vertical mass moment using the floor as the datum. This will be used later to + // calculate max acceleration + Translation3d centerMass = new Translation3d(); + for (Matter object : matter) + { + centerMass = centerMass.plus(object.massMoment()); + } + Translation3d robotCG = centerMass.div(robotMass); + Translation2d horizontalCG = robotCG.toTranslation2d(); + + Translation2d projectedHorizontalCg = + new Translation2d( + (angle.getSin() * angle.getCos() * horizontalCG.getY()) + + (Math.pow(angle.getCos(), 2) * horizontalCG.getX()), + (angle.getSin() * angle.getCos() * horizontalCG.getX()) + + (Math.pow(angle.getSin(), 2) * horizontalCG.getY())); + + // Projects the edge of the wheelbase onto the direction line. Assumes the wheelbase is + // rectangular. + // Because a line is being projected, rather than a point, one of the coordinates of the + // projected point is + // already known. + Translation2d projectedWheelbaseEdge; + double angDeg = angle.getDegrees(); + if (angDeg <= 45 && angDeg >= -45) + { + SwerveModuleConfiguration conf = getSwerveModule(config.modules, true, true); + projectedWheelbaseEdge = + new Translation2d( + conf.moduleLocation.getX(), conf.moduleLocation.getX() * angle.getTan()); + } else if (135 >= angDeg && angDeg > 45) + { + SwerveModuleConfiguration conf = getSwerveModule(config.modules, true, true); + projectedWheelbaseEdge = + new Translation2d( + conf.moduleLocation.getY() / angle.getTan(), conf.moduleLocation.getY()); + } else if (-135 <= angDeg && angDeg < -45) + { + SwerveModuleConfiguration conf = getSwerveModule(config.modules, true, false); + projectedWheelbaseEdge = + new Translation2d( + conf.moduleLocation.getY() / angle.getTan(), conf.moduleLocation.getY()); + } else + { + SwerveModuleConfiguration conf = getSwerveModule(config.modules, false, true); + projectedWheelbaseEdge = + new Translation2d( + conf.moduleLocation.getX(), conf.moduleLocation.getX() * angle.getTan()); + } + + double horizontalDistance = projectedHorizontalCg.plus(projectedWheelbaseEdge).getNorm(); + double maxAccel = 9.81 * horizontalDistance / robotCG.getZ(); + if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) + { + SmartDashboard.putNumber("calcMaxAccel", maxAccel); + } + return maxAccel; + } + + /** + * Logical inverse of the Pose exponential from 254. Taken from team 3181. + * + * @param transform Pose to perform the log on. + * @return {@link Twist2d} of the transformed pose. + */ + public static Twist2d PoseLog(final Pose2d transform) + { + + final double kEps = 1E-9; + final double dtheta = transform.getRotation().getRadians(); + final double half_dtheta = 0.5 * dtheta; + final double cos_minus_one = transform.getRotation().getCos() - 1.0; + double halftheta_by_tan_of_halfdtheta; + if (Math.abs(cos_minus_one) < kEps) + { + halftheta_by_tan_of_halfdtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta; + } else + { + halftheta_by_tan_of_halfdtheta = -(half_dtheta * transform.getRotation().getSin()) / cos_minus_one; + } + final Translation2d translation_part = transform.getTranslation() + .rotateBy(new Rotation2d(halftheta_by_tan_of_halfdtheta, + -half_dtheta)); + return new Twist2d(translation_part.getX(), translation_part.getY(), dtheta); + } + + /** + * Limits a commanded velocity to prevent exceeding the maximum acceleration given by {@link SwerveMath#calcMaxAccel}. + * Note that this takes and returns field-relative velocities. + * + * @param commandedVelocity The desired velocity + * @param fieldVelocity The velocity of the robot within a field relative state. + * @param robotPose The current pose of the robot. + * @param loopTime The time it takes to update the velocity in seconds. Note: this should include the + * 100ms that it takes for a SparkMax velocity to update. + * @param matter Matter that the robot is composed of with position in meters and mass in kg. + * @param robotMass The weight of the robot in kg. (Including manipulators, etc). + * @param config The swerve drive configuration. + * @return The limited velocity. This is either the commanded velocity, if attainable, or the closest attainable + * velocity. + */ + public static Translation2d limitVelocity( + Translation2d commandedVelocity, + ChassisSpeeds fieldVelocity, + Pose2d robotPose, + double loopTime, + double robotMass, + List matter, + SwerveDriveConfiguration config) + { + // Get the robot's current field-relative velocity + Translation2d currentVelocity = SwerveController.getTranslation2d(fieldVelocity); + if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) + { + SmartDashboard.putNumber("currentVelocity", currentVelocity.getX()); + } + + // Calculate the commanded change in velocity by subtracting current velocity + // from commanded velocity + Translation2d deltaV = commandedVelocity.minus(currentVelocity); + if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) + { + SmartDashboard.putNumber("deltaV", deltaV.getX()); + } + + // Creates an acceleration vector with the direction of delta V and a magnitude + // of the maximum allowed acceleration in that direction + Translation2d maxAccel = + new Translation2d( + calcMaxAccel( + deltaV + // Rotates the velocity vector to convert from field-relative to robot-relative + .rotateBy(robotPose.getRotation().unaryMinus()) + .getAngle(), + matter, + robotMass, + config), + deltaV.getAngle()); + + // Calculate the maximum achievable velocity by the next loop cycle. + // delta V = Vf - Vi = at + Translation2d maxAchievableDeltaVelocity = maxAccel.times(loopTime); + + if (deltaV.getNorm() > maxAchievableDeltaVelocity.getNorm()) + { + return maxAchievableDeltaVelocity.plus(currentVelocity); + } else + { + // If the commanded velocity is attainable, use that. + return commandedVelocity; + } + } + + /** + * Get the fruthest module from center based on the module locations. + * + * @param modules Swerve module list. + * @param front True = furthest front, False = furthest back. + * @param left True = furthest left, False = furthest right. + * @return Module location which is the furthest from center and abides by parameters. + */ + public static SwerveModuleConfiguration getSwerveModule( + SwerveModule[] modules, boolean front, boolean left) + { + Translation2d target = modules[0].configuration.moduleLocation, current, temp; + SwerveModuleConfiguration configuration = modules[0].configuration; + for (SwerveModule module : modules) + { + current = module.configuration.moduleLocation; + temp = + front + ? (target.getY() >= current.getY() ? current : target) + : (target.getY() <= current.getY() ? current : target); + target = + left + ? (target.getX() >= temp.getX() ? temp : target) + : (target.getX() <= temp.getX() ? temp : target); + configuration = current.equals(target) ? module.configuration : configuration; + } + return configuration; + } + + /** + * Put an angle within the 360 deg scope of a reference. For example, given a scope reference of 756 degrees, assumes + * the full scope is (720-1080), and places an angle of 22 degrees into it, returning 742 deg. + * + * @param scopeReference Current Angle (deg) + * @param newAngle Target Angle (deg) + * @return Closest angle within scope (deg) + */ + public static double placeInAppropriate0To360Scope(double scopeReference, double newAngle) + { + double lowerBound; + double upperBound; + double lowerOffset = scopeReference % 360; + if (lowerOffset >= 0) + { + lowerBound = scopeReference - lowerOffset; + upperBound = scopeReference + (360 - lowerOffset); + } else + { + upperBound = scopeReference - lowerOffset; + lowerBound = scopeReference - (360 + lowerOffset); + } + while (newAngle < lowerBound) + { + newAngle += 360; + } + while (newAngle > upperBound) + { + newAngle -= 360; + } + if (newAngle - scopeReference > 180) + { + newAngle -= 360; + } else if (newAngle - scopeReference < -180) + { + newAngle += 360; + } + return newAngle; + } + + /** + * Perform anti-jitter within modules if the speed requested is too low. + * + * @param moduleState Current {@link SwerveModuleState} requested. + * @param lastModuleState Previous {@link SwerveModuleState} used. + * @param maxSpeed Maximum speed of the modules. + */ + public static void antiJitter(SwerveModuleState moduleState, SwerveModuleState lastModuleState, double maxSpeed) + { + if (Math.abs(moduleState.speedMetersPerSecond) <= (maxSpeed * 0.01)) + { + moduleState.angle = lastModuleState.angle; + } + } +} diff --git a/src/main/java/swervelib/math/package-info.java b/src/main/java/swervelib/math/package-info.java new file mode 100644 index 00000000..0bfafd54 --- /dev/null +++ b/src/main/java/swervelib/math/package-info.java @@ -0,0 +1,6 @@ +/** + * Mathematics for swerve drives. Original second order kinematics was developed by Team 3181 + * here. + * + */ +package swervelib.math; diff --git a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java new file mode 100644 index 00000000..ba1add7a --- /dev/null +++ b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -0,0 +1,418 @@ +package swervelib.motors; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.ControlType; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame; +import com.revrobotics.REVLibError; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkMaxAlternateEncoder; +import com.revrobotics.SparkMaxPIDController; +import com.revrobotics.SparkMaxRelativeEncoder.Type; +import edu.wpi.first.wpilibj.DriverStation; +import java.util.function.Supplier; +import swervelib.encoders.SwerveAbsoluteEncoder; +import swervelib.parser.PIDFConfig; + +/** + * Brushed motor control with SparkMax. + */ +public class SparkMaxBrushedMotorSwerve extends SwerveMotor +{ + + /** + * SparkMAX Instance. + */ + public CANSparkMax motor; + + /** + * Absolute encoder attached to the SparkMax (if exists) + */ + public AbsoluteEncoder absoluteEncoder; + /** + * Integrated encoder. + */ + public RelativeEncoder encoder; + /** + * Closed-loop PID controller. + */ + public SparkMaxPIDController pid; + /** + * Factory default already occurred. + */ + private boolean factoryDefaultOccurred = false; + + /** + * Initialize the swerve motor. + * + * @param motor The SwerveMotor as a SparkMax object. + * @param isDriveMotor Is the motor being initialized a drive motor? + * @param encoderType {@link Type} of encoder to use for the {@link CANSparkMax} device. + * @param countsPerRevolution The number of encoder pulses for the {@link Type} encoder per revolution. + * @param useDataPortQuadEncoder Use the encoder attached to the data port of the spark max for a quadrature encoder. + */ + public SparkMaxBrushedMotorSwerve(CANSparkMax motor, boolean isDriveMotor, Type encoderType, int countsPerRevolution, + boolean useDataPortQuadEncoder) + { + // Drive motors **MUST** have an encoder attached. + if (isDriveMotor && encoderType == Type.kNoSensor) + { + DriverStation.reportError("Cannot use motor without encoder.", true); + throw new RuntimeException("Cannot use SparkMAX as a drive motor without an encoder attached."); + } + + // Hall encoders can be used as quadrature encoders. + if (encoderType == Type.kHallSensor) + { + encoderType = Type.kQuadrature; + } + + this.motor = motor; + this.isDriveMotor = isDriveMotor; + + factoryDefaults(); + clearStickyFaults(); + + // Get the onboard PID controller. + pid = motor.getPIDController(); + + // If there is a sensor attached to the data port or encoder port set the relative encoder. + if (isDriveMotor || (encoderType != Type.kNoSensor || useDataPortQuadEncoder)) + { + this.encoder = useDataPortQuadEncoder ? + motor.getAlternateEncoder(SparkMaxAlternateEncoder.Type.kQuadrature, countsPerRevolution) : + motor.getEncoder(encoderType, countsPerRevolution); + + // Configure feedback of the PID controller as the integrated encoder. + configureSparkMax(() -> pid.setFeedbackDevice(encoder)); + } + // Spin off configurations in a different thread. + // configureSparkMax(() -> motor.setCANTimeout(0)); // Commented it out because it prevents feedback. + } + + /** + * Initialize the {@link SwerveMotor} as a {@link CANSparkMax} connected to a Brushless Motor. + * + * @param id CAN ID of the SparkMax. + * @param isDriveMotor Is the motor being initialized a drive motor? + * @param encoderType {@link Type} of encoder to use for the {@link CANSparkMax} device. + * @param countsPerRevolution The number of encoder pulses for the {@link Type} encoder per revolution. + * @param useDataPortQuadEncoder Use the encoder attached to the data port of the spark max for a quadrature encoder. + */ + public SparkMaxBrushedMotorSwerve(int id, boolean isDriveMotor, Type encoderType, int countsPerRevolution, + boolean useDataPortQuadEncoder) + { + this(new CANSparkMax(id, MotorType.kBrushed), isDriveMotor, encoderType, countsPerRevolution, + useDataPortQuadEncoder); + } + + /** + * Run the configuration until it succeeds or times out. + * + * @param config Lambda supplier returning the error state. + */ + private void configureSparkMax(Supplier config) + { + for (int i = 0; i < maximumRetries; i++) + { + if (config.get() == REVLibError.kOk) + { + return; + } + } + DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true); + } + + /** + * Set the voltage compensation for the swerve module motor. + * + * @param nominalVoltage Nominal voltage for operation to output to. + */ + @Override + public void setVoltageCompensation(double nominalVoltage) + { + configureSparkMax(() -> motor.enableVoltageCompensation(nominalVoltage)); + } + + /** + * Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with + * voltage compensation. This is useful to protect the motor from current spikes. + * + * @param currentLimit Current limit in AMPS at free speed. + */ + @Override + public void setCurrentLimit(int currentLimit) + { + configureSparkMax(() -> motor.setSmartCurrentLimit(currentLimit)); + } + + /** + * Set the maximum rate the open/closed loop output can change by. + * + * @param rampRate Time in seconds to go from 0 to full throttle. + */ + @Override + public void setLoopRampRate(double rampRate) + { + configureSparkMax(() -> motor.setOpenLoopRampRate(rampRate)); + configureSparkMax(() -> motor.setClosedLoopRampRate(rampRate)); + } + + /** + * Get the motor object from the module. + * + * @return Motor object. + */ + @Override + public Object getMotor() + { + return motor; + } + + /** + * Queries whether the absolute encoder is directly attached to the motor controller. + * + * @return connected absolute encoder state. + */ + @Override + public boolean isAttachedAbsoluteEncoder() + { + return absoluteEncoder != null; + } + + /** + * Configure the factory defaults. + */ + @Override + public void factoryDefaults() + { + if (!factoryDefaultOccurred) + { + configureSparkMax(motor::restoreFactoryDefaults); + factoryDefaultOccurred = true; + } + } + + /** + * Clear the sticky faults on the motor controller. + */ + @Override + public void clearStickyFaults() + { + configureSparkMax(motor::clearFaults); + } + + /** + * Set the absolute encoder to be a compatible absolute encoder. + * + * @param encoder The encoder to use. + * @return The {@link SwerveMotor} for easy instantiation. + */ + @Override + public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) + { + if (encoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) + { + absoluteEncoder = (AbsoluteEncoder) encoder.getAbsoluteEncoder(); + configureSparkMax(() -> pid.setFeedbackDevice(absoluteEncoder)); + } + if (absoluteEncoder == null && this.encoder == null) + { + DriverStation.reportError("An encoder MUST be defined to work with a SparkMAX", true); + throw new RuntimeException("An encoder MUST be defined to work with a SparkMAX"); + } + return this; + } + + /** + * Configure the integrated encoder for the swerve module. Sets the conversion factors for position and velocity. + * + * @param positionConversionFactor The conversion factor to apply. + */ + @Override + public void configureIntegratedEncoder(double positionConversionFactor) + { + if (absoluteEncoder == null) + { + configureSparkMax(() -> encoder.setPositionConversionFactor(positionConversionFactor)); + configureSparkMax(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60)); + + // Taken from + // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67 + configureCANStatusFrames(10, 20, 20, 500, 500); + } else + { + configureSparkMax(() -> absoluteEncoder.setPositionConversionFactor(positionConversionFactor)); + configureSparkMax(() -> absoluteEncoder.setVelocityConversionFactor(positionConversionFactor / 60)); + } + } + + /** + * Configure the PIDF values for the closed loop controller. + * + * @param config Configuration class holding the PIDF values. + */ + @Override + public void configurePIDF(PIDFConfig config) + { +// int pidSlot = +// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal(); + int pidSlot = 0; + + configureSparkMax(() -> pid.setP(config.p, pidSlot)); + configureSparkMax(() -> pid.setI(config.i, pidSlot)); + configureSparkMax(() -> pid.setD(config.d, pidSlot)); + configureSparkMax(() -> pid.setFF(config.f, pidSlot)); + configureSparkMax(() -> pid.setIZone(config.iz, pidSlot)); + configureSparkMax(() -> pid.setOutputRange(config.output.min, config.output.max, pidSlot)); + } + + /** + * Configure the PID wrapping for the position closed loop controller. + * + * @param minInput Minimum PID input. + * @param maxInput Maximum PID input. + */ + @Override + public void configurePIDWrapping(double minInput, double maxInput) + { + configureSparkMax(() -> pid.setPositionPIDWrappingEnabled(true)); + configureSparkMax(() -> pid.setPositionPIDWrappingMinInput(minInput)); + configureSparkMax(() -> pid.setPositionPIDWrappingMaxInput(maxInput)); + } + + /** + * Set the CAN status frames. + * + * @param CANStatus0 Applied Output, Faults, Sticky Faults, Is Follower + * @param CANStatus1 Motor Velocity, Motor Temperature, Motor Voltage, Motor Current + * @param CANStatus2 Motor Position + * @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position + * @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position + */ + public void configureCANStatusFrames( + int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4) + { + configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0)); + configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1)); + configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2)); + configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3)); + configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4)); + // TODO: Configure Status Frame 5 and 6 if necessary + // https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces + } + + /** + * Set the idle mode. + * + * @param isBrakeMode Set the brake mode. + */ + @Override + public void setMotorBrake(boolean isBrakeMode) + { + configureSparkMax(() -> motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast)); + } + + /** + * Set the motor to be inverted. + * + * @param inverted State of inversion. + */ + @Override + public void setInverted(boolean inverted) + { + motor.setInverted(inverted); + } + + /** + * Save the configurations from flash to EEPROM. + */ + @Override + public void burnFlash() + { + configureSparkMax(() -> motor.burnFlash()); + } + + /** + * Set the percentage output. + * + * @param percentOutput percent out for the motor controller. + */ + @Override + public void set(double percentOutput) + { + motor.set(percentOutput); + } + + /** + * Set the closed loop PID controller reference point. + * + * @param setpoint Setpoint in MPS or Angle in degrees. + * @param feedforward Feedforward in volt-meter-per-second or kV. + */ + @Override + public void setReference(double setpoint, double feedforward) + { +// int pidSlot = +// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal(); + int pidSlot = 0; + configureSparkMax(() -> + pid.setReference( + setpoint, + isDriveMotor ? ControlType.kVelocity : ControlType.kPosition, + pidSlot, + feedforward) + ); + } + + /** + * Set the closed loop PID controller reference point. + * + * @param setpoint Setpoint in meters per second or angle in degrees. + * @param feedforward Feedforward in volt-meter-per-second or kV. + * @param position Only used on the angle motor, the position of the motor in degrees. + */ + @Override + public void setReference(double setpoint, double feedforward, double position) + { + setReference(setpoint, feedforward); + } + + /** + * Get the velocity of the integrated encoder. + * + * @return velocity + */ + @Override + public double getVelocity() + { + return absoluteEncoder == null ? encoder.getVelocity() : absoluteEncoder.getVelocity(); + } + + /** + * Get the position of the integrated encoder. + * + * @return Position + */ + @Override + public double getPosition() + { + return absoluteEncoder == null ? encoder.getPosition() : absoluteEncoder.getPosition(); + } + + /** + * Set the integrated encoder position. + * + * @param position Integrated encoder position. + */ + @Override + public void setPosition(double position) + { + if (absoluteEncoder == null) + { + configureSparkMax(() -> encoder.setPosition(position)); + } + } +} diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java new file mode 100644 index 00000000..7ec5c2b8 --- /dev/null +++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java @@ -0,0 +1,441 @@ +package swervelib.motors; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.ControlType; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame; +import com.revrobotics.MotorFeedbackSensor; +import com.revrobotics.REVLibError; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkMaxAnalogSensor; +import com.revrobotics.SparkMaxPIDController; +import edu.wpi.first.wpilibj.DriverStation; +import java.util.function.Supplier; +import swervelib.encoders.SwerveAbsoluteEncoder; +import swervelib.parser.PIDFConfig; + +/** + * An implementation of {@link CANSparkMax} as a {@link SwerveMotor}. + */ +public class SparkMaxSwerve extends SwerveMotor +{ + + /** + * SparkMAX Instance. + */ + public CANSparkMax motor; + /** + * Integrated encoder. + */ + public RelativeEncoder encoder; + /** + * Absolute encoder attached to the SparkMax (if exists) + */ + public SwerveAbsoluteEncoder absoluteEncoder; + /** + * Closed-loop PID controller. + */ + public SparkMaxPIDController pid; + /** + * Factory default already occurred. + */ + private boolean factoryDefaultOccurred = false; + + /** + * Initialize the swerve motor. + * + * @param motor The SwerveMotor as a SparkMax object. + * @param isDriveMotor Is the motor being initialized a drive motor? + */ + public SparkMaxSwerve(CANSparkMax motor, boolean isDriveMotor) + { + this.motor = motor; + this.isDriveMotor = isDriveMotor; + factoryDefaults(); + clearStickyFaults(); + + encoder = motor.getEncoder(); + pid = motor.getPIDController(); + pid.setFeedbackDevice( + encoder); // Configure feedback of the PID controller as the integrated encoder. + + // Spin off configurations in a different thread. + // configureSparkMax(() -> motor.setCANTimeout(0)); // Commented out because it prevents feedback. + } + + /** + * Initialize the {@link SwerveMotor} as a {@link CANSparkMax} connected to a Brushless Motor. + * + * @param id CAN ID of the SparkMax. + * @param isDriveMotor Is the motor being initialized a drive motor? + */ + public SparkMaxSwerve(int id, boolean isDriveMotor) + { + this(new CANSparkMax(id, MotorType.kBrushless), isDriveMotor); + } + + /** + * Run the configuration until it succeeds or times out. + * + * @param config Lambda supplier returning the error state. + */ + private void configureSparkMax(Supplier config) + { + for (int i = 0; i < maximumRetries; i++) + { + if (config.get() == REVLibError.kOk) + { + return; + } + } + DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true); + } + + /** + * Set the voltage compensation for the swerve module motor. + * + * @param nominalVoltage Nominal voltage for operation to output to. + */ + @Override + public void setVoltageCompensation(double nominalVoltage) + { + configureSparkMax(() -> motor.enableVoltageCompensation(nominalVoltage)); + } + + /** + * Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with + * voltage compensation. This is useful to protect the motor from current spikes. + * + * @param currentLimit Current limit in AMPS at free speed. + */ + @Override + public void setCurrentLimit(int currentLimit) + { + configureSparkMax(() -> motor.setSmartCurrentLimit(currentLimit)); + } + + /** + * Set the maximum rate the open/closed loop output can change by. + * + * @param rampRate Time in seconds to go from 0 to full throttle. + */ + @Override + public void setLoopRampRate(double rampRate) + { + configureSparkMax(() -> motor.setOpenLoopRampRate(rampRate)); + configureSparkMax(() -> motor.setClosedLoopRampRate(rampRate)); + } + + /** + * Get the motor object from the module. + * + * @return Motor object. + */ + @Override + public Object getMotor() + { + return motor; + } + + /** + * Queries whether the absolute encoder is directly attached to the motor controller. + * + * @return connected absolute encoder state. + */ + @Override + public boolean isAttachedAbsoluteEncoder() + { + return absoluteEncoder != null; + } + + /** + * Configure the factory defaults. + */ + @Override + public void factoryDefaults() + { + if (!factoryDefaultOccurred) + { + configureSparkMax(motor::restoreFactoryDefaults); + factoryDefaultOccurred = true; + } + } + + /** + * Clear the sticky faults on the motor controller. + */ + @Override + public void clearStickyFaults() + { + configureSparkMax(motor::clearFaults); + } + + /** + * Set the absolute encoder to be a compatible absolute encoder. + * + * @param encoder The encoder to use. + * @return The {@link SwerveMotor} for easy instantiation. + */ + @Override + public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) + { + if (encoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) + { + DriverStation.reportWarning( + "IF possible configure the duty cycle encoder offset in the REV Hardware Client instead of using the" + + " absoluteEncoderOffset in the Swerve Module JSON!", + false); + absoluteEncoder = encoder; + configureSparkMax(() -> pid.setFeedbackDevice((MotorFeedbackSensor) absoluteEncoder.getAbsoluteEncoder())); + } + return this; + } + + /** + * Configure the integrated encoder for the swerve module. Sets the conversion factors for position and velocity. + * + * @param positionConversionFactor The conversion factor to apply. + */ + @Override + public void configureIntegratedEncoder(double positionConversionFactor) + { + if (absoluteEncoder == null) + { + configureSparkMax(() -> encoder.setPositionConversionFactor(positionConversionFactor)); + configureSparkMax(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60)); + + // Taken from + // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67 + configureCANStatusFrames(10, 20, 20, 500, 500); + } else + { + configureSparkMax(() -> { + if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) + { + return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor( + positionConversionFactor); + } else + { + return ((SparkMaxAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor( + positionConversionFactor); + } + }); + configureSparkMax(() -> { + if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) + { + return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor( + positionConversionFactor / 60); + } else + { + return ((SparkMaxAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor( + positionConversionFactor / 60); + } + }); + } + } + + /** + * Configure the PIDF values for the closed loop controller. + * + * @param config Configuration class holding the PIDF values. + */ + @Override + public void configurePIDF(PIDFConfig config) + { +// int pidSlot = +// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal(); + int pidSlot = 0; + configureSparkMax(() -> pid.setP(config.p, pidSlot)); + configureSparkMax(() -> pid.setI(config.i, pidSlot)); + configureSparkMax(() -> pid.setD(config.d, pidSlot)); + configureSparkMax(() -> pid.setFF(config.f, pidSlot)); + configureSparkMax(() -> pid.setIZone(config.iz, pidSlot)); + configureSparkMax(() -> pid.setOutputRange(config.output.min, config.output.max, pidSlot)); + } + + /** + * Configure the PID wrapping for the position closed loop controller. + * + * @param minInput Minimum PID input. + * @param maxInput Maximum PID input. + */ + @Override + public void configurePIDWrapping(double minInput, double maxInput) + { + configureSparkMax(() -> pid.setPositionPIDWrappingEnabled(true)); + configureSparkMax(() -> pid.setPositionPIDWrappingMinInput(minInput)); + configureSparkMax(() -> pid.setPositionPIDWrappingMaxInput(maxInput)); + } + + /** + * Set the CAN status frames. + * + * @param CANStatus0 Applied Output, Faults, Sticky Faults, Is Follower + * @param CANStatus1 Motor Velocity, Motor Temperature, Motor Voltage, Motor Current + * @param CANStatus2 Motor Position + * @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position + * @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position + */ + public void configureCANStatusFrames( + int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4) + { + configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0)); + configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1)); + configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2)); + configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3)); + configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4)); + // TODO: Configure Status Frame 5 and 6 if necessary + // https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces + } + + /** + * Set the idle mode. + * + * @param isBrakeMode Set the brake mode. + */ + @Override + public void setMotorBrake(boolean isBrakeMode) + { + configureSparkMax(() -> motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast)); + } + + /** + * Set the motor to be inverted. + * + * @param inverted State of inversion. + */ + @Override + public void setInverted(boolean inverted) + { + motor.setInverted(inverted); + } + + /** + * Save the configurations from flash to EEPROM. + */ + @Override + public void burnFlash() + { + try + { + Thread.sleep(200); + } catch (Exception e) + { + } + configureSparkMax(() -> motor.burnFlash()); + } + + /** + * Set the percentage output. + * + * @param percentOutput percent out for the motor controller. + */ + @Override + public void set(double percentOutput) + { + motor.set(percentOutput); + } + + /** + * Set the closed loop PID controller reference point. + * + * @param setpoint Setpoint in MPS or Angle in degrees. + * @param feedforward Feedforward in volt-meter-per-second or kV. + */ + @Override + public void setReference(double setpoint, double feedforward) + { + boolean possibleBurnOutIssue = true; +// int pidSlot = +// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal(); + int pidSlot = 0; + + if (isDriveMotor) + { + configureSparkMax(() -> + pid.setReference( + setpoint, + ControlType.kVelocity, + pidSlot, + feedforward)); + } else + { + configureSparkMax(() -> + pid.setReference( + setpoint, + ControlType.kPosition, + pidSlot, + feedforward)); + } + } + + /** + * Set the closed loop PID controller reference point. + * + * @param setpoint Setpoint in meters per second or angle in degrees. + * @param feedforward Feedforward in volt-meter-per-second or kV. + * @param position Only used on the angle motor, the position of the motor in degrees. + */ + @Override + public void setReference(double setpoint, double feedforward, double position) + { + setReference(setpoint, feedforward); + } + + /** + * Get the velocity of the integrated encoder. + * + * @return velocity + */ + @Override + public double getVelocity() + { + return absoluteEncoder == null ? encoder.getVelocity() : absoluteEncoder.getVelocity(); + } + + /** + * Get the position of the integrated encoder. + * + * @return Position + */ + @Override + public double getPosition() + { + return absoluteEncoder == null ? encoder.getPosition() : absoluteEncoder.getAbsolutePosition(); + } + + /** + * Set the integrated encoder position. + * + * @param position Integrated encoder position. + */ + @Override + public void setPosition(double position) + { + if (absoluteEncoder == null) + { + configureSparkMax(() -> encoder.setPosition(position)); + } + } + + /** + * REV Slots for PID configuration. + */ + enum SparkMAX_slotIdx + { + /** + * Slot 1, used for position PID's. + */ + Position, + /** + * Slot 2, used for velocity PID's. + */ + Velocity, + /** + * Slot 3, used arbitrarily. (Documentation show simulations). + */ + Simulation + } +} diff --git a/src/main/java/swervelib/motors/SwerveMotor.java b/src/main/java/swervelib/motors/SwerveMotor.java new file mode 100644 index 00000000..7d585cac --- /dev/null +++ b/src/main/java/swervelib/motors/SwerveMotor.java @@ -0,0 +1,160 @@ +package swervelib.motors; + +import swervelib.encoders.SwerveAbsoluteEncoder; +import swervelib.parser.PIDFConfig; + +/** + * Swerve motor abstraction which defines a standard interface for motors within a swerve module. + */ +public abstract class SwerveMotor +{ + + /** + * The maximum amount of times the swerve motor will attempt to configure a motor if failures occur. + */ + public final int maximumRetries = 5; + /** + * Whether the swerve motor is a drive motor. + */ + protected boolean isDriveMotor; + + /** + * Configure the factory defaults. + */ + public abstract void factoryDefaults(); + + /** + * Clear the sticky faults on the motor controller. + */ + public abstract void clearStickyFaults(); + + /** + * Set the absolute encoder to be a compatible absolute encoder. + * + * @param encoder The encoder to use. + * @return The {@link SwerveMotor} for single line configuration. + */ + public abstract SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder); + + /** + * Configure the integrated encoder for the swerve module. Sets the conversion factors for position and velocity. + * + * @param positionConversionFactor The conversion factor to apply for position. + */ + public abstract void configureIntegratedEncoder(double positionConversionFactor); + + /** + * Configure the PIDF values for the closed loop controller. 0 is disabled or off. + * + * @param config Configuration class holding the PIDF values. + */ + public abstract void configurePIDF(PIDFConfig config); + + /** + * Configure the PID wrapping for the position closed loop controller. + * + * @param minInput Minimum PID input. + * @param maxInput Maximum PID input. + */ + public abstract void configurePIDWrapping(double minInput, double maxInput); + + /** + * Set the idle mode. + * + * @param isBrakeMode Set the brake mode. + */ + public abstract void setMotorBrake(boolean isBrakeMode); + + /** + * Set the motor to be inverted. + * + * @param inverted State of inversion. + */ + public abstract void setInverted(boolean inverted); + + /** + * Save the configurations from flash to EEPROM. + */ + public abstract void burnFlash(); + + /** + * Set the percentage output. + * + * @param percentOutput percent out for the motor controller. + */ + public abstract void set(double percentOutput); + + /** + * Set the closed loop PID controller reference point. + * + * @param setpoint Setpoint in meters per second or angle in degrees. + * @param feedforward Feedforward in volt-meter-per-second or kV. + */ + public abstract void setReference(double setpoint, double feedforward); + + /** + * Set the closed loop PID controller reference point. + * + * @param setpoint Setpoint in meters per second or angle in degrees. + * @param feedforward Feedforward in volt-meter-per-second or kV. + * @param position Only used on the angle motor, the position of the motor in degrees. + */ + public abstract void setReference(double setpoint, double feedforward, double position); + + /** + * Get the velocity of the integrated encoder. + * + * @return velocity in meters per second or degrees per second. + */ + public abstract double getVelocity(); + + /** + * Get the position of the integrated encoder. + * + * @return Position in meters or degrees. + */ + public abstract double getPosition(); + + /** + * Set the integrated encoder position. + * + * @param position Integrated encoder position. Should be angle in degrees or meters per second. + */ + public abstract void setPosition(double position); + + /** + * Set the voltage compensation for the swerve module motor. + * + * @param nominalVoltage Nominal voltage for operation to output to. + */ + public abstract void setVoltageCompensation(double nominalVoltage); + + /** + * Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with + * voltage compensation. This is useful to protect the motor from current spikes. + * + * @param currentLimit Current limit in AMPS at free speed. + */ + public abstract void setCurrentLimit(int currentLimit); + + /** + * Set the maximum rate the open/closed loop output can change by. + * + * @param rampRate Time in seconds to go from 0 to full throttle. + */ + public abstract void setLoopRampRate(double rampRate); + + /** + * Get the motor object from the module. + * + * @return Motor object. + */ + public abstract Object getMotor(); + + /** + * Queries whether the absolute encoder is directly attached to the motor controller. + * + * @return connected absolute encoder state. + */ + public abstract boolean isAttachedAbsoluteEncoder(); +} diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java new file mode 100644 index 00000000..e20b255a --- /dev/null +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -0,0 +1,428 @@ +package swervelib.motors; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import swervelib.encoders.SwerveAbsoluteEncoder; +import swervelib.parser.PIDFConfig; +import swervelib.telemetry.SwerveDriveTelemetry; + +/** + * {@link com.ctre.phoenix6.hardware.TalonFX} Swerve Motor. Made by Team 1466 WebbRobotics. + */ +public class TalonFXSwerve extends SwerveMotor +{ + + /** + * Factory default already occurred. + */ + private final boolean factoryDefaultOccurred = false; + /** + * Current TalonFX configuration. + */ + private TalonFXConfiguration configuration = new TalonFXConfiguration(); + /** + * Whether the absolute encoder is integrated. + */ + private final boolean absoluteEncoder = false; + /** + * Motion magic angle voltage setter. + */ + private final MotionMagicVoltage m_angleVoltageSetter = new MotionMagicVoltage(0); +// /** +// * Motion Magic exponential voltage setters. +// */ +// private final MotionMagicExpoVoltage m_angleVoltageExpoSetter = new MotionMagicExpoVoltage(0); + /** + * Velocity voltage setter for controlling drive motor. + */ + private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0); + /** + * TalonFX motor controller. + */ + TalonFX motor; + + /** + * Constructor for TalonFX swerve motor. + * + * @param motor Motor to use. + * @param isDriveMotor Whether this motor is a drive motor. + */ + public TalonFXSwerve(TalonFX motor, boolean isDriveMotor) + { + this.isDriveMotor = isDriveMotor; + this.motor = motor; + + factoryDefaults(); + clearStickyFaults(); + +// if (SwerveDriveTelemetry.isSimulation) +// { +//// PhysicsSim.getInstance().addTalonFX(motor, .25, 6800); +// } + } + + /** + * Construct the TalonFX swerve motor given the ID and CANBus. + * + * @param id ID of the TalonFX on the CANBus. + * @param canbus CANBus on which the TalonFX is on. + * @param isDriveMotor Whether the motor is a drive or steering motor. + */ + public TalonFXSwerve(int id, String canbus, boolean isDriveMotor) + { + this(new TalonFX(id, canbus), isDriveMotor); + } + + /** + * Construct the TalonFX swerve motor given the ID. + * + * @param id ID of the TalonFX on the canbus. + * @param isDriveMotor Whether the motor is a drive or steering motor. + */ + public TalonFXSwerve(int id, boolean isDriveMotor) + { + this(new TalonFX(id), isDriveMotor); + } + + /** + * Configure the factory defaults. + */ + @Override + public void factoryDefaults() + { + if (!factoryDefaultOccurred) + { + TalonFXConfigurator cfg = motor.getConfigurator(); + configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake; + configuration.ClosedLoopGeneral.ContinuousWrap = true; + cfg.apply(configuration); + + m_angleVoltageSetter.UpdateFreqHz = 0; +// m_angleVoltageExpoSetter.UpdateFreqHz = 0; + m_velocityVoltageSetter.UpdateFreqHz = 0; +// motor.configFactoryDefault(); +// motor.setSensorPhase(true); +// motor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30); +// motor.configNeutralDeadband(0.001); + } + } + + /** + * Clear the sticky faults on the motor controller. + */ + @Override + public void clearStickyFaults() + { + motor.clearStickyFaults(); + } + + /** + * Set the absolute encoder to be a compatible absolute encoder. + * + * @param encoder The encoder to use. + */ + @Override + public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) + { + // Do not support. + return this; + } + + /** + * Configure the integrated encoder for the swerve module. Sets the conversion factors for position and velocity. + * + * @param positionConversionFactor The conversion factor to apply for position. + *


+ * Degrees:
+ * + * 360 / (angleGearRatio * encoderTicksPerRotation) + *
+ *


+ * Meters:
+ * + * (Math.PI * wheelDiameter) / (driveGearRatio * encoderTicksPerRotation) + * + */ + @Override + public void configureIntegratedEncoder(double positionConversionFactor) + { + TalonFXConfigurator cfg = motor.getConfigurator(); + cfg.refresh(configuration); + + configuration.MotionMagic = configuration.MotionMagic + .withMotionMagicCruiseVelocity(100 / positionConversionFactor) + .withMotionMagicAcceleration((100 / positionConversionFactor) / 0.100) + .withMotionMagicExpo_kV(0.12 * positionConversionFactor) + .withMotionMagicExpo_kA(0.1); + + configuration.Feedback = configuration.Feedback + .withSensorToMechanismRatio(positionConversionFactor) + .withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor); + + cfg.apply(configuration); + // Taken from democat's library. + // https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/Falcon500DriveControllerFactoryBuilder.java#L16 + configureCANStatusFrames(250); + } + + /** + * Set the CAN status frames. + * + * @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information + */ + public void configureCANStatusFrames(int CANStatus1) + { +// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); + } + + /** + * Set the CAN status frames. + * + * @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information + * @param CANStatus2 Selected Sensor Position (PID 0), Selected Sensor Velocity (PID 0), Brushed Supply Current + * Measurement, Sticky Fault Information + * @param CANStatus3 Quadrature Information + * @param CANStatus4 Analog Input, Supply Battery Voltage, Controller Temperature + * @param CANStatus8 Pulse Width Information + * @param CANStatus10 Motion Profiling/Motion Magic Information + * @param CANStatus12 Selected Sensor Position (Aux PID 1), Selected Sensor Velocity (Aux PID 1) + * @param CANStatus13 PID0 (Primary PID) Information + * @param CANStatus14 PID1 (Auxiliary PID) Information + * @param CANStatus21 Integrated Sensor Position (Talon FX), Integrated Sensor Velocity (Talon FX) + * @param CANStatusCurrent Brushless Supply Current Measurement, Brushless Stator Current Measurement + */ + public void configureCANStatusFrames(int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4, int CANStatus8, + int CANStatus10, int CANStatus12, int CANStatus13, int CANStatus14, + int CANStatus21, int CANStatusCurrent) + { +// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); +// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, CANStatus2); +// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, CANStatus3); +// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, CANStatus4); +// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, CANStatus8); +// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_Targets, CANStatus10); +// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, CANStatus12); +// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, CANStatus13); +// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, CANStatus14); +// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_21_FeedbackIntegrated, CANStatus21); +// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_Brushless_Current, CANStatusCurrent); + + // TODO: Configure Status Frame 2 thru 21 if necessary + // https://v5.docs.ctr-electronics.com/en/stable/ch18_CommonAPI.html#setting-status-frame-periods + } + + /** + * Configure the PIDF values for the closed loop controller. 0 is disabled or off. + * + * @param config Configuration class holding the PIDF values. + */ + @Override + public void configurePIDF(PIDFConfig config) + { + TalonFXConfigurator cfg = motor.getConfigurator(); + cfg.refresh(configuration.Slot0); + cfg.apply(configuration.Slot0.withKP(config.p) + .withKI(config.i) + .withKD(config.d) + .withKS(config.f)); +// configuration.slot0.integralZone = config.iz; +// configuration.slot0.closedLoopPeakOutput = config.output.max; + } + + /** + * Configure the PID wrapping for the position closed loop controller. + * + * @param minInput Minimum PID input. + * @param maxInput Maximum PID input. + */ + @Override + public void configurePIDWrapping(double minInput, double maxInput) + { + TalonFXConfigurator cfg = motor.getConfigurator(); + cfg.refresh(configuration.ClosedLoopGeneral); + configuration.ClosedLoopGeneral.ContinuousWrap = true; + cfg.apply(configuration.ClosedLoopGeneral); + } + + /** + * Set the idle mode. + * + * @param isBrakeMode Set the brake mode. + */ + @Override + public void setMotorBrake(boolean isBrakeMode) + { + motor.setNeutralMode(isBrakeMode ? NeutralModeValue.Brake : NeutralModeValue.Coast); + } + + /** + * Set the motor to be inverted. + * + * @param inverted State of inversion. + */ + @Override + public void setInverted(boolean inverted) + { +// Timer.delay(1); + motor.setInverted(inverted); + } + + /** + * Save the configurations from flash to EEPROM. + */ + @Override + public void burnFlash() + { + // Do nothing + } + + /** + * Set the percentage output. + * + * @param percentOutput percent out for the motor controller. + */ + @Override + public void set(double percentOutput) + { + motor.set(percentOutput); + } + + /** + * Set the closed loop PID controller reference point. + * + * @param setpoint Setpoint in MPS or Angle in degrees. + * @param feedforward Feedforward in volt-meter-per-second or kV. + */ + @Override + public void setReference(double setpoint, double feedforward) + { + setReference(setpoint, feedforward, getPosition()); + } + + /** + * Set the closed loop PID controller reference point. + * + * @param setpoint Setpoint in meters per second or angle in degrees. + * @param feedforward Feedforward in volt-meter-per-second or kV. + * @param position Only used on the angle motor, the position of the motor in degrees. + */ + @Override + public void setReference(double setpoint, double feedforward, double position) + { +// if (SwerveDriveTelemetry.isSimulation) +// { +// PhysicsSim.getInstance().run(); +// } + + if (isDriveMotor) + { + motor.setControl(m_velocityVoltageSetter.withVelocity(setpoint)); + } else + { + motor.setControl(m_angleVoltageSetter.withPosition(setpoint)); + } + } + + /** + * Get the velocity of the integrated encoder. + * + * @return velocity in Meters Per Second, or Degrees per Second. + */ + @Override + public double getVelocity() + { + return motor.getVelocity().getValue(); + } + + /** + * Get the position of the integrated encoder. + * + * @return Position in Meters or Degrees. + */ + @Override + public double getPosition() + { + return motor.getPosition().getValue(); + } + + /** + * Set the integrated encoder position. + * + * @param position Integrated encoder position. Should be angle in degrees or meters. + */ + @Override + public void setPosition(double position) + { + if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation) + { + position = position < 0 ? (position % 360) + 360 : position; + TalonFXConfigurator cfg = motor.getConfigurator(); + cfg.setPosition(position); + } + } + + /** + * Set the voltage compensation for the swerve module motor. + * + * @param nominalVoltage Nominal voltage for operation to output to. + */ + @Override + public void setVoltageCompensation(double nominalVoltage) + { + // Do not implement + } + + /** + * Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with + * voltage compensation. This is useful to protect the motor from current spikes. + * + * @param currentLimit Current limit in AMPS at free speed. + */ + @Override + public void setCurrentLimit(int currentLimit) + { + TalonFXConfigurator cfg = motor.getConfigurator(); + cfg.refresh(configuration.CurrentLimits); + cfg.apply(configuration.CurrentLimits.withStatorCurrentLimit(currentLimit).withStatorCurrentLimitEnable(true)); + } + + /** + * Set the maximum rate the open/closed loop output can change by. + * + * @param rampRate Time in seconds to go from 0 to full throttle. + */ + @Override + public void setLoopRampRate(double rampRate) + { + TalonFXConfigurator cfg = motor.getConfigurator(); + cfg.refresh(configuration.ClosedLoopRamps); + cfg.apply(configuration.ClosedLoopRamps.withVoltageClosedLoopRampPeriod(rampRate)); + } + + /** + * Get the motor object from the module. + * + * @return Motor object. + */ + @Override + public Object getMotor() + { + return motor; + } + + /** + * Queries whether the absolute encoder is directly attached to the motor controller. + * + * @return connected absolute encoder state. + */ + @Override + public boolean isAttachedAbsoluteEncoder() + { + return absoluteEncoder; + } +} diff --git a/src/main/java/swervelib/motors/TalonSRXSwerve.java b/src/main/java/swervelib/motors/TalonSRXSwerve.java new file mode 100644 index 00000000..0290d828 --- /dev/null +++ b/src/main/java/swervelib/motors/TalonSRXSwerve.java @@ -0,0 +1,415 @@ +package swervelib.motors; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.DemandType; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced; +import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import edu.wpi.first.wpilibj.Timer; +import swervelib.encoders.SwerveAbsoluteEncoder; +import swervelib.math.SwerveMath; +import swervelib.parser.PIDFConfig; +import swervelib.telemetry.SwerveDriveTelemetry; + +/** + * {@link com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX} Swerve Motor. + */ +public class TalonSRXSwerve extends SwerveMotor +{ + + /** + * Factory default already occurred. + */ + private final boolean factoryDefaultOccurred = false; + /** + * Current TalonFX configuration. + */ + private final TalonSRXConfiguration configuration = new TalonSRXConfiguration(); + /** + * Whether the absolute encoder is integrated. + */ + private final boolean absoluteEncoder = false; + /** + * TalonSRX motor controller. + */ + WPI_TalonSRX motor; + /** + * The position conversion factor to convert raw sensor units to Meters Per 100ms, or Ticks to Degrees. + */ + private double positionConversionFactor = 1; + /** + * If the TalonFX configuration has changed. + */ + private boolean configChanged = true; + /** + * Nominal voltage default to use with feedforward. + */ + private double nominalVoltage = 12.0; + + /** + * Constructor for TalonSRX swerve motor. + * + * @param motor Motor to use. + * @param isDriveMotor Whether this motor is a drive motor. + */ + public TalonSRXSwerve(WPI_TalonSRX motor, boolean isDriveMotor) + { + this.isDriveMotor = isDriveMotor; + this.motor = motor; + motor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); + + factoryDefaults(); + clearStickyFaults(); + + } + + /** + * Construct the TalonSRX swerve motor given the ID. + * + * @param id ID of the TalonSRX on the canbus. + * @param isDriveMotor Whether the motor is a drive or steering motor. + */ + public TalonSRXSwerve(int id, boolean isDriveMotor) + { + this(new WPI_TalonSRX(id), isDriveMotor); + } + + /** + * Configure the factory defaults. + */ + @Override + public void factoryDefaults() + { + if (!factoryDefaultOccurred) + { + motor.configFactoryDefault(); + motor.setSensorPhase(true); + } + } + + /** + * Clear the sticky faults on the motor controller. + */ + @Override + public void clearStickyFaults() + { + motor.clearStickyFaults(); + } + + /** + * Set the absolute encoder to be a compatible absolute encoder. + * + * @param encoder The encoder to use. + */ + @Override + public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) + { + // Do not support. + return this; + } + + /** + * Configure the integrated encoder for the swerve module. Sets the conversion factors for position and velocity. + * + * @param positionConversionFactor The conversion factor to apply for position. + *


+ * Degrees:
+ * + * 360 / (angleGearRatio * encoderTicksPerRotation) + *
+ *


+ * Meters:
+ * + * (Math.PI * wheelDiameter) / (driveGearRatio * encoderTicksPerRotation) + * + */ + @Override + public void configureIntegratedEncoder(double positionConversionFactor) + { + this.positionConversionFactor = positionConversionFactor; + // Taken from democat's library. + // https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/Falcon500DriveControllerFactoryBuilder.java#L16 + configureCANStatusFrames(250); + } + + /** + * Set the CAN status frames. + * + * @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information + */ + public void configureCANStatusFrames(int CANStatus1) + { + motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); + } + + /** + * Set the CAN status frames. + * + * @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information + * @param CANStatus2 Selected Sensor Position (PID 0), Selected Sensor Velocity (PID 0), Brushed Supply Current + * Measurement, Sticky Fault Information + * @param CANStatus3 Quadrature Information + * @param CANStatus4 Analog Input, Supply Battery Voltage, Controller Temperature + * @param CANStatus8 Pulse Width Information + * @param CANStatus10 Motion Profiling/Motion Magic Information + * @param CANStatus12 Selected Sensor Position (Aux PID 1), Selected Sensor Velocity (Aux PID 1) + * @param CANStatus13 PID0 (Primary PID) Information + * @param CANStatus14 PID1 (Auxiliary PID) Information + * @param CANStatus21 Integrated Sensor Position (Talon FX), Integrated Sensor Velocity (Talon FX) + * @param CANStatusCurrent Brushless Supply Current Measurement, Brushless Stator Current Measurement + */ + public void configureCANStatusFrames(int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4, int CANStatus8, + int CANStatus10, int CANStatus12, int CANStatus13, int CANStatus14, + int CANStatus21, int CANStatusCurrent) + { + motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); + motor.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, CANStatus2); + motor.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, CANStatus3); + motor.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, CANStatus4); + motor.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, CANStatus8); + motor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_Targets, CANStatus10); + motor.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, CANStatus12); + motor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, CANStatus13); + motor.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, CANStatus14); + motor.setStatusFramePeriod(StatusFrameEnhanced.Status_21_FeedbackIntegrated, CANStatus21); + motor.setStatusFramePeriod(StatusFrameEnhanced.Status_Brushless_Current, CANStatusCurrent); + + // TODO: Configure Status Frame 2 thru 21 if necessary + // https://v5.docs.ctr-electronics.com/en/stable/ch18_CommonAPI.html#setting-status-frame-periods + } + + /** + * Configure the PIDF values for the closed loop controller. 0 is disabled or off. + * + * @param config Configuration class holding the PIDF values. + */ + @Override + public void configurePIDF(PIDFConfig config) + { + configuration.slot0.kP = config.p; + configuration.slot0.kI = config.i; + configuration.slot0.kD = config.d; + configuration.slot0.kF = config.f; + configuration.slot0.integralZone = config.iz; + configuration.slot0.closedLoopPeakOutput = config.output.max; + configChanged = true; + } + + /** + * Configure the PID wrapping for the position closed loop controller. + * + * @param minInput Minimum PID input. + * @param maxInput Maximum PID input. + */ + @Override + public void configurePIDWrapping(double minInput, double maxInput) + { + // Do nothing + } + + /** + * Set the idle mode. + * + * @param isBrakeMode Set the brake mode. + */ + @Override + public void setMotorBrake(boolean isBrakeMode) + { + motor.setNeutralMode(isBrakeMode ? NeutralMode.Brake : NeutralMode.Coast); + } + + /** + * Set the motor to be inverted. + * + * @param inverted State of inversion. + */ + @Override + public void setInverted(boolean inverted) + { + Timer.delay(1); + motor.setInverted(inverted); + } + + /** + * Save the configurations from flash to EEPROM. + */ + @Override + public void burnFlash() + { + if (configChanged) + { + motor.configAllSettings(configuration, 250); + configChanged = false; + } + } + + /** + * Set the percentage output. + * + * @param percentOutput percent out for the motor controller. + */ + @Override + public void set(double percentOutput) + { + motor.set(percentOutput); + } + + + /** + * Convert the setpoint into native sensor units. + * + * @param setpoint Setpoint to mutate. In meters per second or degrees. + * @param position Position in degrees, only used on angle motors. + * @return Setpoint as native sensor units. Encoder ticks per 100ms, or Encoder tick. + */ + public double convertToNativeSensorUnits(double setpoint, double position) + { + setpoint = + isDriveMotor ? setpoint * .1 : SwerveMath.placeInAppropriate0To360Scope(position, setpoint); + return setpoint / positionConversionFactor; + } + + /** + * Set the closed loop PID controller reference point. + * + * @param setpoint Setpoint in MPS or Angle in degrees. + * @param feedforward Feedforward in volt-meter-per-second or kV. + */ + @Override + public void setReference(double setpoint, double feedforward) + { + setReference(setpoint, feedforward, getPosition()); + } + + /** + * Set the closed loop PID controller reference point. + * + * @param setpoint Setpoint in meters per second or angle in degrees. + * @param feedforward Feedforward in volt-meter-per-second or kV. + * @param position Only used on the angle motor, the position of the motor in degrees. + */ + @Override + public void setReference(double setpoint, double feedforward, double position) + { + + burnFlash(); + + motor.set( + isDriveMotor ? ControlMode.Velocity : ControlMode.Position, + convertToNativeSensorUnits(setpoint, position), + DemandType.ArbitraryFeedForward, + feedforward / nominalVoltage); + } + + /** + * Get the velocity of the integrated encoder. + * + * @return velocity in Meters Per Second, or Degrees per Second. + */ + @Override + public double getVelocity() + { + return (motor.getSelectedSensorVelocity() * 10) * positionConversionFactor; + } + + /** + * Get the position of the integrated encoder. + * + * @return Position in Meters or Degrees. + */ + @Override + public double getPosition() + { + if (isDriveMotor) + { + return motor.getSelectedSensorPosition() * positionConversionFactor; + } else + { + var pos = motor.getSelectedSensorPosition() * positionConversionFactor; + pos %= 360; + if (pos < 360) + { + pos += 360; + } + return pos; + } + } + + /** + * Set the integrated encoder position. + * + * @param position Integrated encoder position. Should be angle in degrees or meters. + */ + @Override + public void setPosition(double position) + { + if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation) + { + motor.setSelectedSensorPosition(position / positionConversionFactor, 0, 0); + } + } + + /** + * Set the voltage compensation for the swerve module motor. + * + * @param nominalVoltage Nominal voltage for operation to output to. + */ + @Override + public void setVoltageCompensation(double nominalVoltage) + { + configuration.voltageCompSaturation = nominalVoltage; + configChanged = true; + this.nominalVoltage = nominalVoltage; + } + + /** + * Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with + * voltage compensation. This is useful to protect the motor from current spikes. + * + * @param currentLimit Current limit in AMPS at free speed. + */ + @Override + public void setCurrentLimit(int currentLimit) + { + configuration.continuousCurrentLimit = currentLimit; + configuration.peakCurrentLimit = currentLimit; + configChanged = true; + } + + /** + * Set the maximum rate the open/closed loop output can change by. + * + * @param rampRate Time in seconds to go from 0 to full throttle. + */ + @Override + public void setLoopRampRate(double rampRate) + { + configuration.closedloopRamp = rampRate; + configuration.openloopRamp = rampRate; + configChanged = true; + } + + /** + * Get the motor object from the module. + * + * @return Motor object. + */ + @Override + public Object getMotor() + { + return motor; + } + + /** + * Queries whether the absolute encoder is directly attached to the motor controller. + * + * @return connected absolute encoder state. + */ + @Override + public boolean isAttachedAbsoluteEncoder() + { + return absoluteEncoder; + } +} diff --git a/src/main/java/swervelib/motors/package-info.java b/src/main/java/swervelib/motors/package-info.java new file mode 100644 index 00000000..d8bd36c2 --- /dev/null +++ b/src/main/java/swervelib/motors/package-info.java @@ -0,0 +1,4 @@ +/** + * Swerve motor controller wrappers which implement {@link swervelib.motors.SwerveMotor}. + */ +package swervelib.motors; diff --git a/src/main/java/swervelib/package-info.java b/src/main/java/swervelib/package-info.java new file mode 100644 index 00000000..a9884ca3 --- /dev/null +++ b/src/main/java/swervelib/package-info.java @@ -0,0 +1,6 @@ +/** + * Yet-Another Generic Swerve Library (YAGSL) main package AKA swervelib. + * + * @version 1.0.0 + */ +package swervelib; diff --git a/src/main/java/swervelib/parser/PIDFConfig.java b/src/main/java/swervelib/parser/PIDFConfig.java new file mode 100644 index 00000000..9da06849 --- /dev/null +++ b/src/main/java/swervelib/parser/PIDFConfig.java @@ -0,0 +1,108 @@ +package swervelib.parser; + +import edu.wpi.first.math.controller.PIDController; +import swervelib.parser.deserializer.PIDFRange; + +/** + * Hold the PIDF and Integral Zone values for a PID. + */ +public class PIDFConfig +{ + + /** + * Proportional Gain for PID. + */ + public double p; + /** + * Integral Gain for PID. + */ + public double i; + /** + * Derivative Gain for PID. + */ + public double d; + /** + * Feedforward value for PID. + */ + public double f; + /** + * Integral zone of the PID. + */ + public double iz; + + /** + * The PIDF output range. + */ + public PIDFRange output = new PIDFRange(); + + /** + * Used when parsing PIDF values from JSON. + */ + public PIDFConfig() + { + } + + /** + * PIDF Config constructor to contain the values. + * + * @param p P gain. + * @param i I gain. + * @param d D gain. + * @param f F gain. + * @param iz Intergral zone. + */ + public PIDFConfig(double p, double i, double d, double f, double iz) + { + this.p = p; + this.i = i; + this.d = d; + this.f = f; + this.iz = iz; + } + + /** + * PIDF Config constructor to contain the values. + * + * @param p P gain. + * @param i I gain. + * @param d D gain. + * @param f F gain. + */ + public PIDFConfig(double p, double i, double d, double f) + { + this(p, i, d, f, 0); + } + + /** + * PIDF Config constructor to contain the values. + * + * @param p P gain. + * @param i I gain. + * @param d D gain. + */ + public PIDFConfig(double p, double i, double d) + { + this(p, i, d, 0, 0); + } + + /** + * PIDF Config constructor to contain the values. + * + * @param p P gain. + * @param d D gain. + */ + public PIDFConfig(double p, double d) + { + this(p, 0, d, 0, 0); + } + + /** + * Create a PIDController from the PID values. + * + * @return PIDController. + */ + public PIDController createPIDController() + { + return new PIDController(p, i, d); + } +} diff --git a/src/main/java/swervelib/parser/SwerveControllerConfiguration.java b/src/main/java/swervelib/parser/SwerveControllerConfiguration.java new file mode 100644 index 00000000..7f1d9671 --- /dev/null +++ b/src/main/java/swervelib/parser/SwerveControllerConfiguration.java @@ -0,0 +1,63 @@ +package swervelib.parser; + +import static swervelib.math.SwerveMath.calculateMaxAngularVelocity; + +/** + * Swerve Controller configuration class which is used to configure {@link swervelib.SwerveController}. + */ +public class SwerveControllerConfiguration +{ + + /** + * PIDF for the heading of the robot. + */ + public final PIDFConfig headingPIDF; + /** + * hypotenuse deadband for the robot angle control joystick. + */ + public final double + angleJoyStickRadiusDeadband; // Deadband for the minimum hypot for the heading joystick. + /** + * Maximum angular velocity in rad/s + */ + public double maxAngularVelocity; + + /** + * Construct the swerve controller configuration. Assumes robot is square to fetch maximum angular velocity. + * + * @param driveCfg {@link SwerveDriveConfiguration} to fetch the first module X and Y used to + * calculate the maximum angular velocity. + * @param headingPIDF Heading PIDF configuration. + * @param angleJoyStickRadiusDeadband Deadband on radius of angle joystick. + * @param maxSpeedMPS Maximum speed in meters per second for angular velocity, remember if you have + * feet per second use {@link edu.wpi.first.math.util.Units#feetToMeters(double)}. + */ + public SwerveControllerConfiguration( + SwerveDriveConfiguration driveCfg, + PIDFConfig headingPIDF, + double angleJoyStickRadiusDeadband, + double maxSpeedMPS) + { + this.maxAngularVelocity = + calculateMaxAngularVelocity( + maxSpeedMPS, + Math.abs(driveCfg.moduleLocationsMeters[0].getX()), + Math.abs(driveCfg.moduleLocationsMeters[0].getY())); + this.headingPIDF = headingPIDF; + this.angleJoyStickRadiusDeadband = angleJoyStickRadiusDeadband; + } + + /** + * Construct the swerve controller configuration. Assumes hypotenuse deadband of 0.5 (minimum radius for angle to be + * set on angle joystick is .5 of the controller). + * + * @param driveCfg Drive configuration. + * @param headingPIDF Heading PIDF configuration. + * @param maxSpeedMPS Maximum speed in meters per second for angular velocity, remember if you have feet per second + * use {@link edu.wpi.first.math.util.Units#feetToMeters(double)}. + */ + public SwerveControllerConfiguration(SwerveDriveConfiguration driveCfg, PIDFConfig headingPIDF, double maxSpeedMPS) + { + this(driveCfg, headingPIDF, 0.5, maxSpeedMPS); + } +} diff --git a/src/main/java/swervelib/parser/SwerveDriveConfiguration.java b/src/main/java/swervelib/parser/SwerveDriveConfiguration.java new file mode 100644 index 00000000..f0df58ac --- /dev/null +++ b/src/main/java/swervelib/parser/SwerveDriveConfiguration.java @@ -0,0 +1,95 @@ +package swervelib.parser; + +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Translation2d; +import swervelib.SwerveModule; +import swervelib.imu.SwerveIMU; + +/** + * Swerve drive configurations used during SwerveDrive construction. + */ +public class SwerveDriveConfiguration +{ + + /** + * Swerve Module locations. + */ + public Translation2d[] moduleLocationsMeters; + /** + * Swerve IMU + */ + public SwerveIMU imu; + /** + * Invert the imu measurements. + */ + public boolean invertedIMU = false; + /** + * Number of modules on the robot. + */ + public int moduleCount; + /** + * Swerve Modules. + */ + public SwerveModule[] modules; + /** + * Physical characteristics of the swerve drive from physicalproperties.json. + */ + public SwerveModulePhysicalCharacteristics physicalCharacteristics; + + /** + * Create swerve drive configuration. + * + * @param moduleConfigs Module configuration. + * @param swerveIMU Swerve IMU. + * @param invertedIMU Invert the IMU. + * @param driveFeedforward The drive motor feedforward to use for the {@link SwerveModule}. + * @param physicalCharacteristics {@link SwerveModulePhysicalCharacteristics} to store in association with self. + */ + public SwerveDriveConfiguration( + SwerveModuleConfiguration[] moduleConfigs, + SwerveIMU swerveIMU, + boolean invertedIMU, + SimpleMotorFeedforward driveFeedforward, + SwerveModulePhysicalCharacteristics physicalCharacteristics) + { + this.moduleCount = moduleConfigs.length; + this.imu = swerveIMU; + this.invertedIMU = invertedIMU; + this.modules = createModules(moduleConfigs, driveFeedforward); + this.moduleLocationsMeters = new Translation2d[moduleConfigs.length]; + for (SwerveModule module : modules) + { + this.moduleLocationsMeters[module.moduleNumber] = module.configuration.moduleLocation; + } + this.physicalCharacteristics = physicalCharacteristics; + } + + /** + * Create modules based off of the SwerveModuleConfiguration. + * + * @param swerves Swerve constants. + * @param driveFeedforward Drive feedforward created using + * {@link swervelib.math.SwerveMath#createDriveFeedforward(double, double, double)}. + * @return Swerve Modules. + */ + public SwerveModule[] createModules(SwerveModuleConfiguration[] swerves, SimpleMotorFeedforward driveFeedforward) + { + SwerveModule[] modArr = new SwerveModule[swerves.length]; + for (int i = 0; i < swerves.length; i++) + { + modArr[i] = new SwerveModule(i, swerves[i], driveFeedforward); + } + return modArr; + } + + /** + * Assume the first module is the furthest. Usually front-left. + * + * @return Drive base radius from center of robot to the farthest wheel in meters. + */ + public double getDriveBaseRadiusMeters() + { + Translation2d furthestModule = moduleLocationsMeters[0]; + return Math.abs(Math.sqrt(Math.pow(furthestModule.getX(), 2) + Math.pow(furthestModule.getY(), 2))); + } +} diff --git a/src/main/java/swervelib/parser/SwerveModuleConfiguration.java b/src/main/java/swervelib/parser/SwerveModuleConfiguration.java new file mode 100644 index 00000000..2e8d9f89 --- /dev/null +++ b/src/main/java/swervelib/parser/SwerveModuleConfiguration.java @@ -0,0 +1,162 @@ +package swervelib.parser; + +import edu.wpi.first.math.geometry.Translation2d; +import swervelib.encoders.SwerveAbsoluteEncoder; +import swervelib.motors.SwerveMotor; +import swervelib.parser.json.MotorConfigDouble; + +/** + * Swerve Module configuration class which is used to configure {@link swervelib.SwerveModule}. + */ +public class SwerveModuleConfiguration +{ + + /** + * Conversion factor for drive motor onboard PID's and angle PID's. Use + * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} and + * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)} respectively to calculate the + * conversion factors. + */ + public final MotorConfigDouble conversionFactors; + /** + * Angle offset in degrees for the Swerve Module. + */ + public final double angleOffset; + /** + * Whether the absolute encoder is inverted. + */ + public final boolean absoluteEncoderInverted; + /** + * State of inversion of the drive motor. + */ + public final boolean driveMotorInverted; + /** + * State of inversion of the angle motor. + */ + public final boolean angleMotorInverted; + /** + * PIDF configuration options for the angle motor closed-loop PID controller. + */ + public PIDFConfig anglePIDF; + /** + * PIDF configuration options for the drive motor closed-loop PID controller. + */ + public PIDFConfig velocityPIDF; + /** + * Swerve module location relative to the robot. + */ + public Translation2d moduleLocation; + /** + * Physical characteristics of the swerve module. + */ + public SwerveModulePhysicalCharacteristics physicalCharacteristics; + /** + * The drive motor and angle motor of this swerve module. + */ + public SwerveMotor driveMotor, angleMotor; + /** + * The Absolute Encoder for the swerve module. + */ + public SwerveAbsoluteEncoder absoluteEncoder; + /** + * Name for the swerve module for telemetry. + */ + public String name; + + /** + * Construct a configuration object for swerve modules. + * + * @param driveMotor Drive {@link SwerveMotor}. + * @param angleMotor Angle {@link SwerveMotor} + * @param absoluteEncoder Absolute encoder {@link SwerveAbsoluteEncoder}. + * @param angleOffset Absolute angle offset to 0. + * @param absoluteEncoderInverted Absolute encoder inverted. + * @param angleMotorInverted State of inversion of the angle motor. + * @param driveMotorInverted Drive motor inverted. + * @param xMeters Module location in meters from the center horizontally. + * @param yMeters Module location in meters from center vertically. + * @param anglePIDF Angle PIDF configuration. + * @param velocityPIDF Velocity PIDF configuration. + * @param physicalCharacteristics Physical characteristics of the swerve module. + * @param name The name for the swerve module. + * @param conversionFactors Conversion factors to be applied to the drive and angle motors. + */ + public SwerveModuleConfiguration( + SwerveMotor driveMotor, + SwerveMotor angleMotor, + MotorConfigDouble conversionFactors, + SwerveAbsoluteEncoder absoluteEncoder, + double angleOffset, + double xMeters, + double yMeters, + PIDFConfig anglePIDF, + PIDFConfig velocityPIDF, + SwerveModulePhysicalCharacteristics physicalCharacteristics, + boolean absoluteEncoderInverted, + boolean driveMotorInverted, + boolean angleMotorInverted, + String name) + { + this.driveMotor = driveMotor; + this.angleMotor = angleMotor; + this.conversionFactors = conversionFactors; + this.absoluteEncoder = absoluteEncoder; + this.angleOffset = angleOffset; + this.absoluteEncoderInverted = absoluteEncoderInverted; + this.driveMotorInverted = driveMotorInverted; + this.angleMotorInverted = angleMotorInverted; + this.moduleLocation = new Translation2d(xMeters, yMeters); + this.anglePIDF = anglePIDF; + this.velocityPIDF = velocityPIDF; + this.physicalCharacteristics = physicalCharacteristics; + this.name = name; + } + + /** + * Construct a configuration object for swerve modules. Assumes the absolute encoder and drive motor are not + * inverted. + * + * @param driveMotor Drive {@link SwerveMotor}. + * @param angleMotor Angle {@link SwerveMotor} + * @param conversionFactors Conversion factors for angle/azimuth motors drive factors. + * @param absoluteEncoder Absolute encoder {@link SwerveAbsoluteEncoder}. + * @param angleOffset Absolute angle offset to 0. + * @param xMeters Module location in meters from the center horizontally. + * @param yMeters Module location in meters from center vertically. + * @param anglePIDF Angle PIDF configuration. + * @param velocityPIDF Velocity PIDF configuration. + * @param physicalCharacteristics Physical characteristics of the swerve module. + * @param name Name for the module. + */ + public SwerveModuleConfiguration( + SwerveMotor driveMotor, + SwerveMotor angleMotor, + MotorConfigDouble conversionFactors, + SwerveAbsoluteEncoder absoluteEncoder, + double angleOffset, + double xMeters, + double yMeters, + PIDFConfig anglePIDF, + PIDFConfig velocityPIDF, + SwerveModulePhysicalCharacteristics physicalCharacteristics, + String name) + { + this( + driveMotor, + angleMotor, + conversionFactors, + absoluteEncoder, + angleOffset, + xMeters, + yMeters, + anglePIDF, + velocityPIDF, + physicalCharacteristics, + false, + false, + false, + name); + } + + +} diff --git a/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java b/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java new file mode 100644 index 00000000..99360ce1 --- /dev/null +++ b/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java @@ -0,0 +1,106 @@ +package swervelib.parser; + +import swervelib.parser.json.MotorConfigDouble; + +/** + * Configuration class which stores physical characteristics shared between every swerve module. + */ +public class SwerveModulePhysicalCharacteristics +{ + + /** + * Current limits for the Swerve Module. + */ + public final int driveMotorCurrentLimit, angleMotorCurrentLimit; + /** + * The time it takes for the motor to go from 0 to full throttle in seconds. + */ + public final double driveMotorRampRate, angleMotorRampRate; + /** + * Wheel grip tape coefficient of friction on carpet, as described by the vendor. + */ + public final double wheelGripCoefficientOfFriction; + /** + * The voltage to use for the smart motor voltage compensation. + */ + public double optimalVoltage; + /** + * The conversion factors for the drive and angle motors, created by + * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} and + * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)}. + */ + public MotorConfigDouble conversionFactor; + + /** + * Construct the swerve module physical characteristics. + * + * @param conversionFactors The conversion factors for the drive and angle motors, created by + * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, + * double)} and + * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, + * double)}. + * @param wheelGripCoefficientOfFriction Wheel grip coefficient of friction on carpet given by manufacturer. + * @param optimalVoltage Optimal robot voltage. + * @param driveMotorCurrentLimit Current limit for the drive motor. + * @param angleMotorCurrentLimit Current limit for the angle motor. + * @param driveMotorRampRate The time in seconds to go from 0 to full throttle on the motor. (Prevents + * over drawing power from battery) + * @param angleMotorRampRate The time in seconds to go from 0 to full throttle on the motor. (Prevents + * overdrawing power and power loss). + */ + public SwerveModulePhysicalCharacteristics( + MotorConfigDouble conversionFactors, + double wheelGripCoefficientOfFriction, + double optimalVoltage, + int driveMotorCurrentLimit, + int angleMotorCurrentLimit, + double driveMotorRampRate, + double angleMotorRampRate) + { + this.wheelGripCoefficientOfFriction = wheelGripCoefficientOfFriction; + this.optimalVoltage = optimalVoltage; + + this.conversionFactor = conversionFactors; + // Set the conversion factors to null if they are both 0. + if (conversionFactors != null) + { + if (conversionFactors.angle == 0 && conversionFactors.drive == 0) + { + this.conversionFactor = null; + } + } + + this.driveMotorCurrentLimit = driveMotorCurrentLimit; + this.angleMotorCurrentLimit = angleMotorCurrentLimit; + this.driveMotorRampRate = driveMotorRampRate; + this.angleMotorRampRate = angleMotorRampRate; + } + + /** + * Construct the swerve module physical characteristics. Assume coefficient of friction is 1.19 (taken from blue + * nitrile on carpet from Studica) and optimal voltage is 12v. Assumes the drive motor current limit is 40A, and the + * angle motor current limit is 20A. + * + * @param conversionFactors The conversion factors for the drive and angle motors, created by + * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} and + * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)}. + * @param driveMotorRampRate The time in seconds to go from 0 to full throttle on the motor. (Prevents over drawing + * power from battery) + * @param angleMotorRampRate The time in seconds to go from 0 to full throttle on the motor. (Prevents overdrawing + * power and power loss). + */ + public SwerveModulePhysicalCharacteristics( + MotorConfigDouble conversionFactors, + double driveMotorRampRate, + double angleMotorRampRate) + { + this( + conversionFactors, + 1.19, + 12, + 40, + 20, + driveMotorRampRate, + angleMotorRampRate); + } +} diff --git a/src/main/java/swervelib/parser/SwerveParser.java b/src/main/java/swervelib/parser/SwerveParser.java new file mode 100644 index 00000000..573fdd87 --- /dev/null +++ b/src/main/java/swervelib/parser/SwerveParser.java @@ -0,0 +1,227 @@ +package swervelib.parser; + +import com.fasterxml.jackson.databind.JsonNode; +import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import java.io.File; +import java.io.IOException; +import java.util.HashMap; +import swervelib.SwerveDrive; +import swervelib.SwerveModule; +import swervelib.math.SwerveMath; +import swervelib.parser.json.ControllerPropertiesJson; +import swervelib.parser.json.ModuleJson; +import swervelib.parser.json.PIDFPropertiesJson; +import swervelib.parser.json.PhysicalPropertiesJson; +import swervelib.parser.json.SwerveDriveJson; + +/** + * Helper class used to parse the JSON directory with specified configuration options. + */ +public class SwerveParser +{ + + /** + * Module number mapped to the JSON name. + */ + private static final HashMap moduleConfigs = new HashMap<>(); + /** + * Parsed swervedrive.json + */ + public static SwerveDriveJson swerveDriveJson; + /** + * Parsed controllerproperties.json + */ + public static ControllerPropertiesJson controllerPropertiesJson; + /** + * Parsed modules/pidfproperties.json + */ + public static PIDFPropertiesJson pidfPropertiesJson; + /** + * Parsed modules/physicalproperties.json + */ + public static PhysicalPropertiesJson physicalPropertiesJson; + /** + * Array holding the module jsons given in {@link SwerveDriveJson}. + */ + public static ModuleJson[] moduleJsons; + + /** + * Construct a swerve parser. Will throw an error if there is a missing file. + * + * @param directory Directory with swerve configurations. + * @throws IOException if a file doesn't exist. + */ + public SwerveParser(File directory) throws IOException + { + checkDirectory(directory); + swerveDriveJson = + new ObjectMapper() + .readValue(new File(directory, "swervedrive.json"), SwerveDriveJson.class); + controllerPropertiesJson = + new ObjectMapper() + .readValue( + new File(directory, "controllerproperties.json"), ControllerPropertiesJson.class); + pidfPropertiesJson = + new ObjectMapper() + .readValue( + new File(directory, "modules/pidfproperties.json"), PIDFPropertiesJson.class); + physicalPropertiesJson = + new ObjectMapper() + .readValue( + new File(directory, "modules/physicalproperties.json"), + PhysicalPropertiesJson.class); + moduleJsons = new ModuleJson[swerveDriveJson.modules.length]; + for (int i = 0; i < moduleJsons.length; i++) + { + moduleConfigs.put(swerveDriveJson.modules[i], i); + File moduleFile = new File(directory, "modules/" + swerveDriveJson.modules[i]); + assert moduleFile.exists(); + moduleJsons[i] = new ObjectMapper().readValue(moduleFile, ModuleJson.class); + } + } + + /** + * Get the swerve module by the json name. + * + * @param name JSON name. + * @param driveConfiguration {@link SwerveDriveConfiguration} to pull from. + * @return {@link SwerveModuleConfiguration} based on the file. + */ + public static SwerveModule getModuleConfigurationByName( + String name, SwerveDriveConfiguration driveConfiguration) + { + return driveConfiguration.modules[moduleConfigs.get(name + ".json")]; + } + + /** + * Open JSON file. + * + * @param file JSON File to open. + * @return JsonNode of file. + */ + private JsonNode openJson(File file) + { + try + { + return new ObjectMapper().readTree(file); + } catch (IOException e) + { + throw new RuntimeException(e); + } + } + + /** + * Check directory structure. + * + * @param directory JSON Configuration Directory + */ + private void checkDirectory(File directory) + { + assert new File(directory, "swervedrive.json").exists(); + assert new File(directory, "controllerproperties.json").exists(); + assert new File(directory, "modules").exists() && new File(directory, "modules").isDirectory(); + assert new File(directory, "modules/pidfproperties.json").exists(); + assert new File(directory, "modules/physicalproperties.json").exists(); + } + + /** + * Create {@link SwerveDrive} from JSON configuration directory. + * + * @param maxSpeed Maximum speed of the robot in meters per second, used for both angular acceleration used in + * {@link swervelib.SwerveController} and drive feedforward in + * {@link SwerveMath#createDriveFeedforward(double, double, double)}. + * @return {@link SwerveDrive} instance. + */ + public SwerveDrive createSwerveDrive(double maxSpeed) + { + return createSwerveDrive(SwerveMath.createDriveFeedforward(physicalPropertiesJson.optimalVoltage, + maxSpeed, + physicalPropertiesJson.wheelGripCoefficientOfFriction), + maxSpeed); + } + + /** + * Create {@link SwerveDrive} from JSON configuration directory. + * + * @param maxSpeed Maximum speed of the robot in meters per second, used for both angular + * acceleration used in {@link swervelib.SwerveController} and drive feedforward in + * {@link SwerveMath#createDriveFeedforward(double, double, double)}. + * @param angleMotorConversionFactor Angle (AKA azimuth) motor conversion factor to convert motor controller PID loop + * units to degrees, usually created using + * {@link SwerveMath#calculateDegreesPerSteeringRotation(double, double)}. + * @param driveMotorConversion Drive motor conversion factor to convert motor controller PID loop units to + * meters per rotation, usually created using + * {@link SwerveMath#calculateMetersPerRotation(double, double, double)}. + * @return {@link SwerveDrive} instance. + */ + public SwerveDrive createSwerveDrive(double maxSpeed, double angleMotorConversionFactor, double driveMotorConversion) + { + physicalPropertiesJson.conversionFactor.angle = angleMotorConversionFactor; + physicalPropertiesJson.conversionFactor.drive = driveMotorConversion; + return createSwerveDrive(SwerveMath.createDriveFeedforward(physicalPropertiesJson.optimalVoltage, + maxSpeed, + physicalPropertiesJson.wheelGripCoefficientOfFriction), + maxSpeed); + } + + /** + * Create {@link SwerveDrive} from JSON configuration directory. + * + * @param driveFeedforward Drive feedforward to use for swerve modules, should be created using + * {@link swervelib.math.SwerveMath#createDriveFeedforward(double, double, + * double)}. + * @param maxSpeed Maximum speed of the robot in meters per second for normal+angular acceleration + * in {@link swervelib.SwerveController} of the robot. + * @param angleMotorConversionFactor Angle (AKA azimuth) motor conversion factor to convert motor controller PID loop + * units to degrees, usually created using + * {@link SwerveMath#calculateDegreesPerSteeringRotation(double, double)}. + * @param driveMotorConversion Drive motor conversion factor to convert motor controller PID loop units to + * meters per rotation, usually created using + * {@link SwerveMath#calculateMetersPerRotation(double, double, double)}. + * @return {@link SwerveDrive} instance. + */ + public SwerveDrive createSwerveDrive(SimpleMotorFeedforward driveFeedforward, double maxSpeed, + double angleMotorConversionFactor, double driveMotorConversion) + { + physicalPropertiesJson.conversionFactor.angle = angleMotorConversionFactor; + physicalPropertiesJson.conversionFactor.drive = driveMotorConversion; + return createSwerveDrive(driveFeedforward, maxSpeed); + } + + /** + * Create {@link SwerveDrive} from JSON configuration directory. + * + * @param driveFeedforward Drive feedforward to use for swerve modules, should be created using + * {@link swervelib.math.SwerveMath#createDriveFeedforward(double, double, double)}. + * @param maxSpeed Maximum speed of the robot in meters per second for normal+angular acceleration in + * {@link swervelib.SwerveController} of the robot + * @return {@link SwerveDrive} instance. + */ + public SwerveDrive createSwerveDrive(SimpleMotorFeedforward driveFeedforward, double maxSpeed) + { + SwerveModuleConfiguration[] moduleConfigurations = + new SwerveModuleConfiguration[moduleJsons.length]; + for (int i = 0; i < moduleConfigurations.length; i++) + { + ModuleJson module = moduleJsons[i]; + moduleConfigurations[i] = + module.createModuleConfiguration( + pidfPropertiesJson.angle, + pidfPropertiesJson.drive, + physicalPropertiesJson.createPhysicalProperties(), + swerveDriveJson.modules[i]); + } + SwerveDriveConfiguration swerveDriveConfiguration = + new SwerveDriveConfiguration( + moduleConfigurations, + swerveDriveJson.imu.createIMU(), + swerveDriveJson.invertedIMU, + driveFeedforward, + physicalPropertiesJson.createPhysicalProperties()); + + return new SwerveDrive( + swerveDriveConfiguration, + controllerPropertiesJson.createControllerConfiguration(swerveDriveConfiguration, maxSpeed), maxSpeed); + } +} diff --git a/src/main/java/swervelib/parser/deserializer/PIDFRange.java b/src/main/java/swervelib/parser/deserializer/PIDFRange.java new file mode 100644 index 00000000..c90f2312 --- /dev/null +++ b/src/main/java/swervelib/parser/deserializer/PIDFRange.java @@ -0,0 +1,17 @@ +package swervelib.parser.deserializer; + +/** + * Class to hold the minimum and maximum input or output of the PIDF. + */ +public class PIDFRange +{ + + /** + * Minimum value. + */ + public double min = -1; + /** + * Maximum value. + */ + public double max = 1; +} diff --git a/src/main/java/swervelib/parser/deserializer/package-info.java b/src/main/java/swervelib/parser/deserializer/package-info.java new file mode 100644 index 00000000..820f2cc0 --- /dev/null +++ b/src/main/java/swervelib/parser/deserializer/package-info.java @@ -0,0 +1,4 @@ +/** + * Deserialize specific variables for outside the parser. + */ +package swervelib.parser.deserializer; diff --git a/src/main/java/swervelib/parser/json/ControllerPropertiesJson.java b/src/main/java/swervelib/parser/json/ControllerPropertiesJson.java new file mode 100644 index 00000000..103107db --- /dev/null +++ b/src/main/java/swervelib/parser/json/ControllerPropertiesJson.java @@ -0,0 +1,35 @@ +package swervelib.parser.json; + +import swervelib.parser.PIDFConfig; +import swervelib.parser.SwerveControllerConfiguration; +import swervelib.parser.SwerveDriveConfiguration; + +/** + * {@link swervelib.SwerveController} parsed class. Used to access the JSON data. + */ +public class ControllerPropertiesJson +{ + + /** + * The minimum radius of the angle control joystick to allow for heading adjustment of the robot. + */ + public double angleJoystickRadiusDeadband; + /** + * The PID used to control the robot heading. + */ + public PIDFConfig heading; + + /** + * Create the {@link SwerveControllerConfiguration} based on parsed and given data. + * + * @param driveConfiguration {@link SwerveDriveConfiguration} parsed configuration. + * @param maxSpeedMPS Maximum speed in meters per second for the angular acceleration of the robot. + * @return {@link SwerveControllerConfiguration} object based on parsed data. + */ + public SwerveControllerConfiguration createControllerConfiguration( + SwerveDriveConfiguration driveConfiguration, double maxSpeedMPS) + { + return new SwerveControllerConfiguration( + driveConfiguration, heading, angleJoystickRadiusDeadband, maxSpeedMPS); + } +} diff --git a/src/main/java/swervelib/parser/json/DeviceJson.java b/src/main/java/swervelib/parser/json/DeviceJson.java new file mode 100644 index 00000000..9f130d51 --- /dev/null +++ b/src/main/java/swervelib/parser/json/DeviceJson.java @@ -0,0 +1,187 @@ +package swervelib.parser.json; + +import com.revrobotics.SparkMaxRelativeEncoder.Type; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.I2C; +import edu.wpi.first.wpilibj.SPI; +import edu.wpi.first.wpilibj.SerialPort.Port; +import swervelib.encoders.AnalogAbsoluteEncoderSwerve; +import swervelib.encoders.CANCoderSwerve; +import swervelib.encoders.CanAndCoderSwerve; +import swervelib.encoders.PWMDutyCycleEncoderSwerve; +import swervelib.encoders.SparkMaxAnalogEncoderSwerve; +import swervelib.encoders.SparkMaxEncoderSwerve; +import swervelib.encoders.SwerveAbsoluteEncoder; +import swervelib.imu.ADIS16448Swerve; +import swervelib.imu.ADIS16470Swerve; +import swervelib.imu.ADXRS450Swerve; +import swervelib.imu.AnalogGyroSwerve; +import swervelib.imu.NavXSwerve; +import swervelib.imu.Pigeon2Swerve; +import swervelib.imu.PigeonSwerve; +import swervelib.imu.SwerveIMU; +import swervelib.motors.SparkMaxBrushedMotorSwerve; +import swervelib.motors.SparkMaxSwerve; +import swervelib.motors.SwerveMotor; +import swervelib.motors.TalonFXSwerve; +import swervelib.motors.TalonSRXSwerve; + +/** + * Device JSON parsed class. Used to access the JSON data. + */ +public class DeviceJson +{ + + /** + * The device type, e.g. pigeon/pigeon2/sparkmax/talonfx/navx + */ + public String type; + /** + * The CAN ID or pin ID of the device. + */ + public int id; + /** + * The CAN bus name which the device resides on if using CAN. + */ + public String canbus = ""; + + /** + * Create a {@link SwerveAbsoluteEncoder} from the current configuration. + * + * @param motor {@link SwerveMotor} of which attached encoders will be created from, only used when the type is + * "attached" or "canandencoder". + * @return {@link SwerveAbsoluteEncoder} given. + */ + public SwerveAbsoluteEncoder createEncoder(SwerveMotor motor) + { + if (id > 40) + { + DriverStation.reportWarning("CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!", + false); + } + switch (type) + { + case "none": + return null; + case "integrated": + case "attached": + return new SparkMaxEncoderSwerve(motor, 1); + case "sparkmax_analog": + return new SparkMaxAnalogEncoderSwerve(motor); + case "canandcoder": + return new SparkMaxEncoderSwerve(motor, 360); + case "canandcoder_can": + return new CanAndCoderSwerve(id); + case "ma3": + case "ctre_mag": + case "rev_hex": + case "throughbore": + case "am_mag": + case "dutycycle": + return new PWMDutyCycleEncoderSwerve(id); + case "thrifty": + case "analog": + return new AnalogAbsoluteEncoderSwerve(id); + case "cancoder": + return new CANCoderSwerve(id, canbus != null ? canbus : ""); + default: + throw new RuntimeException(type + " is not a recognized absolute encoder type."); + } + } + + /** + * Create a {@link SwerveIMU} from the given configuration. + * + * @return {@link SwerveIMU} given. + */ + public SwerveIMU createIMU() + { + if (id > 40) + { + DriverStation.reportWarning("CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!", + false); + } + switch (type) + { + case "adis16448": + return new ADIS16448Swerve(); + case "adis16470": + return new ADIS16470Swerve(); + case "adxrs450": + return new ADXRS450Swerve(); + case "analog": + return new AnalogGyroSwerve(id); + case "navx": + case "navx_spi": + return new NavXSwerve(SPI.Port.kMXP); + case "navx_i2c": + DriverStation.reportWarning( + "WARNING: There exists an I2C lockup issue on the roboRIO that could occur, more information here: " + + "\nhttps://docs.wpilib.org/en/stable/docs/yearly-overview/known-issues" + + ".html#onboard-i2c-causing-system-lockups", + false); + return new NavXSwerve(I2C.Port.kMXP); + case "navx_usb": + return new NavXSwerve(Port.kUSB); + case "navx_mxp": + return new NavXSwerve(Port.kMXP); + case "pigeon": + return new PigeonSwerve(id); + case "pigeon2": + return new Pigeon2Swerve(id, canbus != null ? canbus : ""); + default: + throw new RuntimeException(type + " is not a recognized imu/gyroscope type."); + } + } + + /** + * Create a {@link SwerveMotor} from the given configuration. + * + * @param isDriveMotor If the motor being generated is a drive motor. + * @return {@link SwerveMotor} given. + */ + public SwerveMotor createMotor(boolean isDriveMotor) + { + if (id > 40) + { + DriverStation.reportWarning("CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!", + false); + } + switch (type) + { + case "sparkmax_brushed": + switch (canbus) + { + case "greyhill_63r256": + return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 1024, false); + case "srx_mag_encoder": + return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 4096, false); + case "throughbore": + return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 8192, false); + case "throughbore_dataport": + return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kNoSensor, 8192, true); + case "greyhill_63r256_dataport": + return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 1024, true); + case "srx_mag_encoder_dataport": + return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 4096, true); + default: + if (isDriveMotor) + { + throw new RuntimeException("Spark MAX " + id + " MUST have a encoder attached to the motor controller."); + } + // We are creating a motor for an angle motor which will use the absolute encoder attached to the data port. + return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kNoSensor, 0, false); + } + case "neo": + case "sparkmax": + return new SparkMaxSwerve(id, isDriveMotor); + case "falcon": + case "talonfx": + return new TalonFXSwerve(id, canbus != null ? canbus : "", isDriveMotor); + case "talonsrx": + return new TalonSRXSwerve(id, isDriveMotor); + default: + throw new RuntimeException(type + " is not a recognized motor type."); + } + } +} diff --git a/src/main/java/swervelib/parser/json/ModuleJson.java b/src/main/java/swervelib/parser/json/ModuleJson.java new file mode 100644 index 00000000..0d4f4906 --- /dev/null +++ b/src/main/java/swervelib/parser/json/ModuleJson.java @@ -0,0 +1,137 @@ +package swervelib.parser.json; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.MotorFeedbackSensor; +import edu.wpi.first.math.util.Units; +import swervelib.encoders.SwerveAbsoluteEncoder; +import swervelib.motors.SwerveMotor; +import swervelib.parser.PIDFConfig; +import swervelib.parser.SwerveModuleConfiguration; +import swervelib.parser.SwerveModulePhysicalCharacteristics; +import swervelib.parser.json.modules.BoolMotorJson; +import swervelib.parser.json.modules.LocationJson; + +/** + * {@link swervelib.SwerveModule} JSON parsed class. Used to access the JSON data. + */ +public class ModuleJson +{ + + /** + * Drive motor device configuration. + */ + public DeviceJson drive; + /** + * Angle motor device configuration. + */ + public DeviceJson angle; + /** + * Conversion factor for the module, if different from the one in swervedrive.json + *

+ * Conversion factor applied to the motor controllers PID loops. Can be calculated with + * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)} for angle motors or + * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} for drive motors. + */ + public MotorConfigDouble conversionFactor = new MotorConfigDouble(0, 0); + /** + * Absolute encoder device configuration. + */ + public DeviceJson encoder; + /** + * Defines which motors are inverted. + */ + public BoolMotorJson inverted; + /** + * Absolute encoder offset from 0 in degrees. + */ + public double absoluteEncoderOffset; + /** + * Absolute encoder inversion state. + */ + public boolean absoluteEncoderInverted = false; + /** + * The location of the swerve module from the center of the robot in inches. + */ + public LocationJson location; + + /** + * Create the swerve module configuration based off of parsed data. + * + * @param anglePIDF The PIDF values for the angle motor. + * @param velocityPIDF The velocity PIDF values for the drive motor. + * @param physicalCharacteristics Physical characteristics of the swerve module. + * @param name Module json filename. + * @return {@link SwerveModuleConfiguration} based on the provided data and parsed data. + */ + public SwerveModuleConfiguration createModuleConfiguration( + PIDFConfig anglePIDF, + PIDFConfig velocityPIDF, + SwerveModulePhysicalCharacteristics physicalCharacteristics, + String name) + { + SwerveMotor angleMotor = angle.createMotor(false); + SwerveAbsoluteEncoder absEncoder = encoder.createEncoder(angleMotor); + + // If the absolute encoder is attached. + if (absEncoder != null && angleMotor.getMotor() instanceof CANSparkMax) + { + if (absEncoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) + { + angleMotor.setAbsoluteEncoder(absEncoder); + } + } + + // Set the conversion factors to null if they are both 0. + if (this.conversionFactor != null) + { + if (this.conversionFactor.angle == 0 && this.conversionFactor.drive == 0) + { + this.conversionFactor = null; + } + } + + if (this.conversionFactor == null && physicalCharacteristics.conversionFactor == null) + { + throw new RuntimeException("No Conversion Factor configured! Please create SwerveDrive using \n" + + "SwerveParser.createSwerveDrive(driveFeedforward, maxSpeed, angleMotorConversionFactor, driveMotorConversion)\n" + + "OR\n" + + "SwerveParser.createSwerveDrive(maxSpeed, angleMotorConversionFactor, driveMotorConversion)\n" + + "OR\n" + + "Set the conversion factor in physicalproperties.json OR the module JSON file." + + "REMEMBER: You can calculate the conversion factors using SwerveMath.calculateMetersPerRotation AND SwerveMath.calculateDegreesPerSteeringRotation\n"); + } else if (physicalCharacteristics.conversionFactor != null && this.conversionFactor == null) + { + this.conversionFactor = physicalCharacteristics.conversionFactor; + } else if (physicalCharacteristics.conversionFactor != + null) // If both are defined, override 0 with the physical characterstics input. + { + this.conversionFactor.angle = this.conversionFactor.angle == 0 ? physicalCharacteristics.conversionFactor.angle + : this.conversionFactor.angle; + this.conversionFactor.drive = this.conversionFactor.drive == 0 ? physicalCharacteristics.conversionFactor.drive + : this.conversionFactor.drive; + } + + if (this.conversionFactor.drive == 0 || this.conversionFactor.angle == 0) + { + throw new RuntimeException( + "Conversion factors cannot be 0, please configure conversion factors in physicalproperties.json or the module JSON files."); + } + System.out.println(conversionFactor.drive); + + return new SwerveModuleConfiguration( + drive.createMotor(true), + angleMotor, + conversionFactor, + absEncoder, + absoluteEncoderOffset, + Units.inchesToMeters(Math.round(location.x) == 0 ? location.front : location.x), + Units.inchesToMeters(Math.round(location.y) == 0 ? location.left : location.y), + anglePIDF, + velocityPIDF, + physicalCharacteristics, + absoluteEncoderInverted, + inverted.drive, + inverted.angle, + name.replaceAll("\\.json", "")); + } +} diff --git a/src/main/java/swervelib/parser/json/MotorConfigDouble.java b/src/main/java/swervelib/parser/json/MotorConfigDouble.java new file mode 100644 index 00000000..86766b3b --- /dev/null +++ b/src/main/java/swervelib/parser/json/MotorConfigDouble.java @@ -0,0 +1,36 @@ +package swervelib.parser.json; + +/** + * Used to store doubles for motor configuration. + */ +public class MotorConfigDouble +{ + + /** + * Drive motor. + */ + public double drive; + /** + * Angle motor. + */ + public double angle; + + /** + * Default constructor. + */ + public MotorConfigDouble() + { + } + + /** + * Default constructor. + * + * @param angle Angle data. + * @param drive Drive data. + */ + public MotorConfigDouble(double angle, double drive) + { + this.angle = angle; + this.drive = drive; + } +} diff --git a/src/main/java/swervelib/parser/json/MotorConfigInt.java b/src/main/java/swervelib/parser/json/MotorConfigInt.java new file mode 100644 index 00000000..d31726c1 --- /dev/null +++ b/src/main/java/swervelib/parser/json/MotorConfigInt.java @@ -0,0 +1,36 @@ +package swervelib.parser.json; + +/** + * Used to store ints for motor configuration. + */ +public class MotorConfigInt +{ + + /** + * Drive motor. + */ + public int drive; + /** + * Angle motor. + */ + public int angle; + + /** + * Default constructor. + */ + public MotorConfigInt() + { + } + + /** + * Default constructor with values. + * + * @param drive Drive data. + * @param angle Angle data. + */ + public MotorConfigInt(int drive, int angle) + { + this.angle = angle; + this.drive = drive; + } +} diff --git a/src/main/java/swervelib/parser/json/PIDFPropertiesJson.java b/src/main/java/swervelib/parser/json/PIDFPropertiesJson.java new file mode 100644 index 00000000..9dd9dc0c --- /dev/null +++ b/src/main/java/swervelib/parser/json/PIDFPropertiesJson.java @@ -0,0 +1,19 @@ +package swervelib.parser.json; + +import swervelib.parser.PIDFConfig; + +/** + * {@link swervelib.SwerveModule} PID with Feedforward for the drive motor and angle motor. + */ +public class PIDFPropertiesJson +{ + + /** + * The PIDF with Integral Zone used for the drive motor. + */ + public PIDFConfig drive; + /** + * The PIDF with Integral Zone used for the angle motor. + */ + public PIDFConfig angle; +} diff --git a/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java b/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java new file mode 100644 index 00000000..4ede4025 --- /dev/null +++ b/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java @@ -0,0 +1,52 @@ +package swervelib.parser.json; + +import swervelib.parser.SwerveModulePhysicalCharacteristics; + +/** + * {@link swervelib.parser.SwerveModulePhysicalCharacteristics} parsed data. Used to configure the SwerveModule. + */ +public class PhysicalPropertiesJson +{ + + + /** + * Conversion factor applied to the motor controllers PID loops. Can be calculated with + * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)} for angle motors or + * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} for drive motors. + */ + public MotorConfigDouble conversionFactor = new MotorConfigDouble(0, 0); + /** + * The current limit in AMPs to apply to the motors. + */ + public MotorConfigInt currentLimit = new MotorConfigInt(40, 20); + /** + * The minimum number of seconds to take for the motor to go from 0 to full throttle. + */ + public MotorConfigDouble rampRate = new MotorConfigDouble(0.25, 0.25); + /** + * The grip tape coefficient of friction on carpet. Used to calculate the practical maximum acceleration. + */ + public double wheelGripCoefficientOfFriction = 1.19; + /** + * The voltage to use for the smart motor voltage compensation, default is 12. + */ + public double optimalVoltage = 12; + + /** + * Create the physical characteristics based off the parsed data. + * + * @return {@link SwerveModulePhysicalCharacteristics} based on parsed data. + */ + public SwerveModulePhysicalCharacteristics createPhysicalProperties() + { + return new SwerveModulePhysicalCharacteristics( + conversionFactor, + wheelGripCoefficientOfFriction, + optimalVoltage, + currentLimit.drive, + currentLimit.angle, + rampRate.drive, + rampRate.angle); + } +} + diff --git a/src/main/java/swervelib/parser/json/SwerveDriveJson.java b/src/main/java/swervelib/parser/json/SwerveDriveJson.java new file mode 100644 index 00000000..332d30d7 --- /dev/null +++ b/src/main/java/swervelib/parser/json/SwerveDriveJson.java @@ -0,0 +1,21 @@ +package swervelib.parser.json; + +/** + * {@link swervelib.SwerveDrive} JSON parsed class. Used to access parsed data from the swervedrive.json file. + */ +public class SwerveDriveJson +{ + + /** + * Robot IMU used to determine heading of the robot. + */ + public DeviceJson imu; + /** + * Invert the IMU of the robot. + */ + public boolean invertedIMU; + /** + * Module JSONs in order clockwise order starting from front left. + */ + public String[] modules; +} diff --git a/src/main/java/swervelib/parser/json/modules/BoolMotorJson.java b/src/main/java/swervelib/parser/json/modules/BoolMotorJson.java new file mode 100644 index 00000000..f6d603f5 --- /dev/null +++ b/src/main/java/swervelib/parser/json/modules/BoolMotorJson.java @@ -0,0 +1,17 @@ +package swervelib.parser.json.modules; + +/** + * Inverted motor JSON parsed class. Used to access the JSON data. + */ +public class BoolMotorJson +{ + + /** + * Drive motor inversion state. + */ + public boolean drive; + /** + * Angle motor inversion state. + */ + public boolean angle; +} diff --git a/src/main/java/swervelib/parser/json/modules/LocationJson.java b/src/main/java/swervelib/parser/json/modules/LocationJson.java new file mode 100644 index 00000000..a0471efb --- /dev/null +++ b/src/main/java/swervelib/parser/json/modules/LocationJson.java @@ -0,0 +1,18 @@ +package swervelib.parser.json.modules; + +/** + * Location JSON parsed class. Used to access the JSON data. Module locations, in inches, as distances to the center of + * the robot. +x is towards the robot front, and +y is towards robot left. + */ +public class LocationJson +{ + + /** + * Location of the swerve module in inches from the center of the robot horizontally. + */ + public double front = 0, x = 0; + /** + * Location of the swerve module in inches from the center of the robot vertically. + */ + public double left = 0, y = 0; +} diff --git a/src/main/java/swervelib/parser/json/modules/package-info.java b/src/main/java/swervelib/parser/json/modules/package-info.java new file mode 100644 index 00000000..b5ff2b71 --- /dev/null +++ b/src/main/java/swervelib/parser/json/modules/package-info.java @@ -0,0 +1,4 @@ +/** + * JSON Mapped Configuration types for modules. + */ +package swervelib.parser.json.modules; diff --git a/src/main/java/swervelib/parser/json/package-info.java b/src/main/java/swervelib/parser/json/package-info.java new file mode 100644 index 00000000..836fbc0e --- /dev/null +++ b/src/main/java/swervelib/parser/json/package-info.java @@ -0,0 +1,4 @@ +/** + * JSON Mapped classes for parsing configuration files. + */ +package swervelib.parser.json; diff --git a/src/main/java/swervelib/parser/package-info.java b/src/main/java/swervelib/parser/package-info.java new file mode 100644 index 00000000..ade6ed38 --- /dev/null +++ b/src/main/java/swervelib/parser/package-info.java @@ -0,0 +1,4 @@ +/** + * JSON Parser for YAGSL configurations. + */ +package swervelib.parser; diff --git a/src/main/java/swervelib/simulation/SwerveIMUSimulation.java b/src/main/java/swervelib/simulation/SwerveIMUSimulation.java new file mode 100644 index 00000000..b3f2b62d --- /dev/null +++ b/src/main/java/swervelib/simulation/SwerveIMUSimulation.java @@ -0,0 +1,122 @@ +package swervelib.simulation; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import java.util.Optional; + +/** + * Simulation for {@link swervelib.SwerveDrive} IMU. + */ +public class SwerveIMUSimulation +{ + + /** + * Main timer to control movement estimations. + */ + private final Timer timer; + /** + * The last time the timer was read, used to determine position changes. + */ + private double lastTime; + /** + * Heading of the robot. + */ + private double angle; + + /** + * Create the swerve drive IMU simulation. + */ + public SwerveIMUSimulation() + { + timer = new Timer(); + timer.start(); + lastTime = timer.get(); + } + + /** + * Get the estimated angle of the robot. + * + * @return {@link Rotation2d} estimation of the robot. + */ + public Rotation2d getYaw() + { + return new Rotation2d(angle); + } + + /** + * Pitch is not simulated currently, always returns 0. + * + * @return Pitch of the robot as {@link Rotation2d}. + */ + public Rotation2d getPitch() + { + return new Rotation2d(); + } + + /** + * Roll is not simulated currently, always returns 0. + * + * @return Roll of the robot as {@link Rotation2d}. + */ + public Rotation2d getRoll() + { + return new Rotation2d(); + } + + /** + * Gets the estimated gyro {@link Rotation3d} of the robot. + * + * @return The heading as a {@link Rotation3d} angle + */ + public Rotation3d getGyroRotation3d() + { + return new Rotation3d(0, 0, angle); + } + + /** + * Fetch the acceleration [x, y, z] from the IMU in m/s/s. If acceleration isn't supported returns empty. + * + * @return {@link Translation3d} of the acceleration as an {@link Optional}. + */ + public Optional getAccel() + { + return Optional.empty(); + } + + /** + * Update the odometry of the simulated {@link swervelib.SwerveDrive} and post the {@link swervelib.SwerveModule} + * states to the {@link Field2d}. + * + * @param kinematics {@link SwerveDriveKinematics} of the swerve drive. + * @param states {@link SwerveModuleState} array of the module states. + * @param modulePoses {@link Pose2d} representing the swerve modules. + * @param field {@link Field2d} to update. + */ + public void updateOdometry( + SwerveDriveKinematics kinematics, + SwerveModuleState[] states, + Pose2d[] modulePoses, + Field2d field) + { + + angle += kinematics.toChassisSpeeds(states).omegaRadiansPerSecond * (timer.get() - lastTime); + lastTime = timer.get(); + field.getObject("XModules").setPoses(modulePoses); + } + + /** + * Set the heading of the robot. + * + * @param angle Angle of the robot in radians. + */ + public void setAngle(double angle) + { + this.angle = angle; + } +} diff --git a/src/main/java/swervelib/simulation/SwerveModuleSimulation.java b/src/main/java/swervelib/simulation/SwerveModuleSimulation.java new file mode 100644 index 00000000..2fb06f8f --- /dev/null +++ b/src/main/java/swervelib/simulation/SwerveModuleSimulation.java @@ -0,0 +1,91 @@ +package swervelib.simulation; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.Timer; + +/** + * Class to hold simulation data for {@link swervelib.SwerveModule} + */ +public class SwerveModuleSimulation +{ + + + /** + * Main timer to simulate the passage of time. + */ + private final Timer timer; + /** + * Time delta since last update + */ + private double dt; + /** + * Fake motor position. + */ + private double fakePos; + /** + * The fake speed of the previous state, used to calculate {@link SwerveModuleSimulation#fakePos}. + */ + private double fakeSpeed; + /** + * Last time queried. + */ + private double lastTime; + /** + * Current simulated swerve module state. + */ + private SwerveModuleState state; + + /** + * Create simulation class and initialize module at 0. + */ + public SwerveModuleSimulation() + { + timer = new Timer(); + timer.start(); + lastTime = timer.get(); + state = new SwerveModuleState(0, Rotation2d.fromDegrees(0)); + fakeSpeed = 0; + fakePos = 0; + dt = 0; + } + + /** + * Update the position and state of the module. Called from {@link swervelib.SwerveModule#setDesiredState} function + * when simulated. + * + * @param desiredState State the swerve module is set to. + */ + public void updateStateAndPosition(SwerveModuleState desiredState) + { + dt = timer.get() - lastTime; + lastTime = timer.get(); + + state = desiredState; + fakeSpeed = desiredState.speedMetersPerSecond; + fakePos += (fakeSpeed * dt); + + } + + /** + * Get the simulated swerve module position. + * + * @return {@link SwerveModulePosition} of the simulated module. + */ + public SwerveModulePosition getPosition() + { + + return new SwerveModulePosition(fakePos, state.angle); + } + + /** + * Get the {@link SwerveModuleState} of the simulated module. + * + * @return {@link SwerveModuleState} of the simulated module. + */ + public SwerveModuleState getState() + { + return state; + } +} diff --git a/src/main/java/swervelib/simulation/package-info.java b/src/main/java/swervelib/simulation/package-info.java new file mode 100644 index 00000000..9aa4aece --- /dev/null +++ b/src/main/java/swervelib/simulation/package-info.java @@ -0,0 +1,4 @@ +/** + * Classes used to simulate the swerve drive. + */ +package swervelib.simulation; diff --git a/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java b/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java new file mode 100644 index 00000000..7c1f5461 --- /dev/null +++ b/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java @@ -0,0 +1,118 @@ +package swervelib.telemetry; + +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * Telemetry to describe the {@link swervelib.SwerveDrive} following frc-web-components. (Which follows AdvantageKit) + */ +public class SwerveDriveTelemetry +{ + + /** + * The current telemetry verbosity level. + */ + public static TelemetryVerbosity verbosity = TelemetryVerbosity.MACHINE; + /** + * State of simulation of the Robot, used to optimize retrieval. + */ + public static boolean isSimulation = RobotBase.isSimulation(); + /** + * The number of swerve modules + */ + public static int moduleCount; + /** + * The number of swerve modules + */ + public static double[] wheelLocations; + /** + * An array of rotation and velocity values describing the measured state of each swerve module + */ + public static double[] measuredStates; + /** + * An array of rotation and velocity values describing the desired state of each swerve module + */ + public static double[] desiredStates; + /** + * The robot's current rotation based on odometry or gyro readings + */ + public static double robotRotation = 0; + /** + * The maximum achievable speed of the modules, used to adjust the size of the vectors. + */ + public static double maxSpeed; + /** + * The units of the module rotations and robot rotation + */ + public static String rotationUnit = "degrees"; + /** + * The distance between the left and right modules. + */ + public static double sizeLeftRight; + /** + * The distance between the front and back modules. + */ + public static double sizeFrontBack; + /** + * The direction the robot should be facing when the "Robot Rotation" is zero or blank. This option is often useful to + * align with odometry data or match videos. 'up', 'right', 'down' or 'left' + */ + public static String forwardDirection = "up"; + /** + * The maximum achievable angular velocity of the robot. This is used to visualize the angular velocity from the + * chassis speeds properties. + */ + public static double maxAngularVelocity; + /** + * The maximum achievable angular velocity of the robot. This is used to visualize the angular velocity from the + * chassis speeds properties. + */ + public static double[] measuredChassisSpeeds = new double[3]; + /** + * Describes the desired forward, sideways and angular velocity of the robot. + */ + public static double[] desiredChassisSpeeds = new double[3]; + + /** + * Upload data to smartdashboard + */ + public static void updateData() + { + SmartDashboard.putNumber("swerve/moduleCount", moduleCount); + SmartDashboard.putNumberArray("swerve/wheelLocations", wheelLocations); + SmartDashboard.putNumberArray("swerve/measuredStates", measuredStates); + SmartDashboard.putNumberArray("swerve/desiredStates", desiredStates); + SmartDashboard.putNumber("swerve/robotRotation", robotRotation); + SmartDashboard.putNumber("swerve/maxSpeed", maxSpeed); + SmartDashboard.putString("swerve/rotationUnit", rotationUnit); + SmartDashboard.putNumber("swerve/sizeLeftRight", sizeLeftRight); + SmartDashboard.putNumber("swerve/sizeFrontBack", sizeFrontBack); + SmartDashboard.putString("swerve/forwardDirection", forwardDirection); + SmartDashboard.putNumber("swerve/maxAngularVelocity", maxAngularVelocity); + SmartDashboard.putNumberArray("swerve/measuredChassisSpeeds", measuredChassisSpeeds); + SmartDashboard.putNumberArray("swerve/desiredChassisSpeeds", desiredChassisSpeeds); + } + + /** + * Verbosity of telemetry data sent back. + */ + public enum TelemetryVerbosity + { + /** + * No telemetry data is sent back. + */ + NONE, + /** + * Low telemetry data, only post the robot position on the field. + */ + LOW, + /** + * Full swerve drive data is sent back in both human and machine readable forms. + */ + HIGH, + /** + * Only send the machine readable data related to swerve drive. + */ + MACHINE + } +} diff --git a/src/main/java/swervelib/telemetry/package-info.java b/src/main/java/swervelib/telemetry/package-info.java new file mode 100644 index 00000000..4b8f6112 --- /dev/null +++ b/src/main/java/swervelib/telemetry/package-info.java @@ -0,0 +1,4 @@ +/** + * Telemetry package for sending data to NT4 or SmartDashboard. + */ +package swervelib.telemetry; \ No newline at end of file diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json deleted file mode 100644 index 3e18a45d..00000000 --- a/vendordeps/PathplannerLib.json +++ /dev/null @@ -1,38 +0,0 @@ -{ - "fileName": "PathplannerLib.json", - "name": "PathplannerLib", - "version": "2024.0.0-beta-6.2", - "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", - "frcYear": "2024", - "mavenUrls": [ - "https://3015rangerrobotics.github.io/pathplannerlib/repo" - ], - "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", - "javaDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-java", - "version": "2024.0.0-beta-6.2" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-cpp", - "version": "2024.0.0-beta-6.2", - "libName": "PathplannerLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal", - "linuxathena", - "linuxarm32", - "linuxarm64" - ] - } - ] -} \ No newline at end of file diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json deleted file mode 100644 index ebbff7d2..00000000 --- a/vendordeps/Phoenix6.json +++ /dev/null @@ -1,339 +0,0 @@ -{ - "fileName": "Phoenix6.json", - "name": "CTRE-Phoenix (v6)", - "version": "24.0.0-beta-7", - "frcYear": 2024, - "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", - "mavenUrls": [ - "https://maven.ctr-electronics.com/release/" - ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-beta-latest.json", - "conflictsWith": [ - { - "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", - "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", - "offlineFileName": "Phoenix6And5.json" - } - ], - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-java", - "version": "24.0.0-beta-7" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "24.0.0-beta-7", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "24.0.0-beta-7", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "24.0.0-beta-7", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonFX", - "version": "24.0.0-beta-7", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "24.0.0-beta-7", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "24.0.0-beta-7", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "24.0.0-beta-7", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "24.0.0-beta-7", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "24.0.0-beta-7", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "24.0.0-beta-7", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-cpp", - "version": "24.0.0-beta-7", - "libName": "CTRE_Phoenix6_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "24.0.0-beta-7", - "libName": "CTRE_PhoenixTools", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "24.0.0-beta-7", - "libName": "CTRE_Phoenix6_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "24.0.0-beta-7", - "libName": "CTRE_PhoenixTools_Sim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "24.0.0-beta-7", - "libName": "CTRE_SimTalonSRX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonFX", - "version": "24.0.0-beta-7", - "libName": "CTRE_SimTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "24.0.0-beta-7", - "libName": "CTRE_SimVictorSPX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "24.0.0-beta-7", - "libName": "CTRE_SimPigeonIMU", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "24.0.0-beta-7", - "libName": "CTRE_SimCANCoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "24.0.0-beta-7", - "libName": "CTRE_SimProTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "24.0.0-beta-7", - "libName": "CTRE_SimProCANcoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "24.0.0-beta-7", - "libName": "CTRE_SimProPigeon2", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ] -} \ No newline at end of file diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json index e0a8b434..67bf3898 100644 --- a/vendordeps/WPILibNewCommands.json +++ b/vendordeps/WPILibNewCommands.json @@ -1,37 +1,38 @@ { - "fileName": "WPILibNewCommands.json", - "name": "WPILib-New-Commands", - "version": "1.0.0", - "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", - "mavenUrls": [], - "jsonUrl": "", - "javaDependencies": [ - { - "groupId": "edu.wpi.first.wpilibNewCommands", - "artifactId": "wpilibNewCommands-java", - "version": "wpilib" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "edu.wpi.first.wpilibNewCommands", - "artifactId": "wpilibNewCommands-cpp", - "version": "wpilib", - "libName": "wpilibNewCommands", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxraspbian", - "linuxaarch64bionic", - "windowsx86-64", - "windowsx86", - "linuxx86-64", - "osxx86-64" - ] - } - ] -} \ No newline at end of file + "fileName": "WPILibNewCommands.json", + "name": "WPILib-New-Commands", + "version": "1.0.0", + "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", + "frcYear": "2024", + "mavenUrls": [], + "jsonUrl": "", + "javaDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-java", + "version": "wpilib" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-cpp", + "version": "wpilib", + "libName": "wpilibNewCommands", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "osxuniversal" + ] + } + ] +} diff --git a/vendordeps/yagsl.json b/vendordeps/yagsl.json deleted file mode 100644 index b5ece1a5..00000000 --- a/vendordeps/yagsl.json +++ /dev/null @@ -1,19 +0,0 @@ -{ - "fileName": "yagsl.json", - "name": "YAGSL", - "version": "2023.2.9", - "uuid": "1ccce5a4-acd2-4d18-bca3-4b8047188400", - "mavenUrls": [ - "https://broncbotz3481.github.io/YAGSL-Lib/yagsl/repos" - ], - "jsonUrl": "https://broncbotz3481.github.io/YAGSL-Lib/yagsl/yagsl.json", - "javaDependencies": [ - { - "groupId": "swervelib", - "artifactId": "YAGSL-java", - "version": "2023.2.9" - } - ], - "jniDependencies": [], - "cppDependencies": [] -} \ No newline at end of file From 9b2634ce19b8fd6977eb2bee14f86d13217f8696 Mon Sep 17 00:00:00 2001 From: DriverStation3 Date: Mon, 8 Jan 2024 20:24:09 -0800 Subject: [PATCH 05/12] update things to 2024 release --- build.gradle | 2 +- networktables.json | 1 + simgui-ds.json | 92 +++++ simgui.json | 20 ++ .../robot/subsystems/DrivebaseSubsystem.java | 22 +- ...IS16470Swerve.java => ADIS16470Swerve.txt} | 0 .../{PigeonSwerve.java => PigeonSwerve.txt} | 0 ...ve.java => SparkMaxBrushedMotorSwerve.txt} | 0 ...SparkMaxSwerve.java => SparkMaxSwerve.txt} | 0 ...TalonSRXSwerve.java => TalonSRXSwerve.txt} | 0 .../swervelib/parser/json/DeviceJson.java | 35 +- vendordeps/NavX.json | 40 +++ vendordeps/PathplannerLib.json | 38 ++ vendordeps/Phoenix6.json | 339 ++++++++++++++++++ vendordeps/REVLib.json | 74 ++++ vendordeps/ReduxLib_2023.json | 55 +++ 16 files changed, 692 insertions(+), 26 deletions(-) create mode 100644 networktables.json create mode 100644 simgui-ds.json create mode 100644 simgui.json rename src/main/java/swervelib/imu/{ADIS16470Swerve.java => ADIS16470Swerve.txt} (100%) rename src/main/java/swervelib/imu/{PigeonSwerve.java => PigeonSwerve.txt} (100%) rename src/main/java/swervelib/motors/{SparkMaxBrushedMotorSwerve.java => SparkMaxBrushedMotorSwerve.txt} (100%) rename src/main/java/swervelib/motors/{SparkMaxSwerve.java => SparkMaxSwerve.txt} (100%) rename src/main/java/swervelib/motors/{TalonSRXSwerve.java => TalonSRXSwerve.txt} (100%) create mode 100644 vendordeps/NavX.json create mode 100644 vendordeps/PathplannerLib.json create mode 100644 vendordeps/Phoenix6.json create mode 100644 vendordeps/REVLib.json create mode 100644 vendordeps/ReduxLib_2023.json diff --git a/build.gradle b/build.gradle index a8ca9c5b..78f4933c 100644 --- a/build.gradle +++ b/build.gradle @@ -8,7 +8,7 @@ java { targetCompatibility = JavaVersion.VERSION_17 } -def ROBOT_MAIN_CLASS = "" +def ROBOT_MAIN_CLASS = "frc.team2412.robot.Main" // Define my targets (RoboRIO) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. diff --git a/networktables.json b/networktables.json new file mode 100644 index 00000000..fe51488c --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 00000000..73cc713c --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 00000000..89168d8e --- /dev/null +++ b/simgui.json @@ -0,0 +1,20 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/Field": "Field2d", + "/SmartDashboard/Scheduler": "Scheduler", + "/SmartDashboard/navX-Sensor[1]": "Gyro" + }, + "windows": { + "/SmartDashboard/Scheduler": { + "window": { + "visible": true + } + } + } + }, + "NetworkTables Info": { + "visible": true + } +} diff --git a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java index 1b24c8c3..7ce7c955 100644 --- a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java @@ -8,7 +8,9 @@ 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.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.team2412.robot.Robot; @@ -75,7 +77,7 @@ public DrivebaseSubsystem() { MAX_AUTO_SPEED, DRIVEBASE_RADIUS, new ReplanningConfig()), - false, + ()->DriverStation.getAlliance().get().equals(Alliance.Red), // flip path if on the red alliance this); } @@ -108,14 +110,16 @@ public void drive(Translation2d translation, Rotation2d rotation, boolean fieldO */ public Command driveJoystick( DoubleSupplier forward, DoubleSupplier strafe, Supplier rotation) { - Rotation2d constrainedRotation = - Rotation2d.fromRotations( - SwerveMath.applyDeadband(rotation.get().getRotations(), true, JOYSTICK_DEADBAND)); - Translation2d constrainedTranslation = - new Translation2d( - SwerveMath.applyDeadband(forward.getAsDouble(), true, JOYSTICK_DEADBAND), - SwerveMath.applyDeadband(strafe.getAsDouble(), true, JOYSTICK_DEADBAND)); - return this.run(() -> drive(constrainedTranslation, constrainedRotation, true)); + return this.run(() -> { + Rotation2d constrainedRotation = + Rotation2d.fromRotations( + SwerveMath.applyDeadband(rotation.get().getRotations(), true, JOYSTICK_DEADBAND)); + Translation2d constrainedTranslation = + new Translation2d( + SwerveMath.applyDeadband(forward.getAsDouble(), true, JOYSTICK_DEADBAND), + SwerveMath.applyDeadband(strafe.getAsDouble(), true, JOYSTICK_DEADBAND)); + drive(constrainedTranslation, constrainedRotation, true); + }); } public ChassisSpeeds getRobotSpeeds() { diff --git a/src/main/java/swervelib/imu/ADIS16470Swerve.java b/src/main/java/swervelib/imu/ADIS16470Swerve.txt similarity index 100% rename from src/main/java/swervelib/imu/ADIS16470Swerve.java rename to src/main/java/swervelib/imu/ADIS16470Swerve.txt diff --git a/src/main/java/swervelib/imu/PigeonSwerve.java b/src/main/java/swervelib/imu/PigeonSwerve.txt similarity index 100% rename from src/main/java/swervelib/imu/PigeonSwerve.java rename to src/main/java/swervelib/imu/PigeonSwerve.txt diff --git a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.txt similarity index 100% rename from src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java rename to src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.txt diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.txt similarity index 100% rename from src/main/java/swervelib/motors/SparkMaxSwerve.java rename to src/main/java/swervelib/motors/SparkMaxSwerve.txt diff --git a/src/main/java/swervelib/motors/TalonSRXSwerve.java b/src/main/java/swervelib/motors/TalonSRXSwerve.txt similarity index 100% rename from src/main/java/swervelib/motors/TalonSRXSwerve.java rename to src/main/java/swervelib/motors/TalonSRXSwerve.txt diff --git a/src/main/java/swervelib/parser/json/DeviceJson.java b/src/main/java/swervelib/parser/json/DeviceJson.java index 9f130d51..ab5d86fb 100644 --- a/src/main/java/swervelib/parser/json/DeviceJson.java +++ b/src/main/java/swervelib/parser/json/DeviceJson.java @@ -13,18 +13,18 @@ import swervelib.encoders.SparkMaxEncoderSwerve; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.imu.ADIS16448Swerve; -import swervelib.imu.ADIS16470Swerve; +// import swervelib.imu.ADIS16470Swerve; import swervelib.imu.ADXRS450Swerve; import swervelib.imu.AnalogGyroSwerve; import swervelib.imu.NavXSwerve; import swervelib.imu.Pigeon2Swerve; -import swervelib.imu.PigeonSwerve; +// import swervelib.imu.PigeonSwerve; import swervelib.imu.SwerveIMU; -import swervelib.motors.SparkMaxBrushedMotorSwerve; -import swervelib.motors.SparkMaxSwerve; +// import swervelib.motors.SparkMaxBrushedMotorSwerve; +// import swervelib.motors.SparkMaxSwerve; import swervelib.motors.SwerveMotor; import swervelib.motors.TalonFXSwerve; -import swervelib.motors.TalonSRXSwerve; +// import swervelib.motors.TalonSRXSwerve; /** * Device JSON parsed class. Used to access the JSON data. @@ -106,7 +106,8 @@ public SwerveIMU createIMU() case "adis16448": return new ADIS16448Swerve(); case "adis16470": - return new ADIS16470Swerve(); + // return new ADIS16470Swerve(); + return null; case "adxrs450": return new ADXRS450Swerve(); case "analog": @@ -126,7 +127,8 @@ public SwerveIMU createIMU() case "navx_mxp": return new NavXSwerve(Port.kMXP); case "pigeon": - return new PigeonSwerve(id); + // return new PigeonSwerve(id); + return null; case "pigeon2": return new Pigeon2Swerve(id, canbus != null ? canbus : ""); default: @@ -153,33 +155,34 @@ public SwerveMotor createMotor(boolean isDriveMotor) switch (canbus) { case "greyhill_63r256": - return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 1024, false); + // return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 1024, false); case "srx_mag_encoder": - return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 4096, false); + // return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 4096, false); case "throughbore": - return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 8192, false); + // return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 8192, false); case "throughbore_dataport": - return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kNoSensor, 8192, true); + // return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kNoSensor, 8192, true); case "greyhill_63r256_dataport": - return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 1024, true); + // return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 1024, true); case "srx_mag_encoder_dataport": - return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 4096, true); + // return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 4096, true); default: if (isDriveMotor) { throw new RuntimeException("Spark MAX " + id + " MUST have a encoder attached to the motor controller."); } // We are creating a motor for an angle motor which will use the absolute encoder attached to the data port. - return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kNoSensor, 0, false); + // return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kNoSensor, 0, false); + return null; } case "neo": case "sparkmax": - return new SparkMaxSwerve(id, isDriveMotor); + // return new SparkMaxSwerve(id, isDriveMotor); case "falcon": case "talonfx": return new TalonFXSwerve(id, canbus != null ? canbus : "", isDriveMotor); case "talonsrx": - return new TalonSRXSwerve(id, isDriveMotor); + // return new TalonSRXSwerve(id, isDriveMotor); default: throw new RuntimeException(type + " is not a recognized motor type."); } diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json new file mode 100644 index 00000000..e978a5f7 --- /dev/null +++ b/vendordeps/NavX.json @@ -0,0 +1,40 @@ +{ + "fileName": "NavX.json", + "name": "NavX", + "version": "2024.1.0", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "frcYear": "2024", + "mavenUrls": [ + "https://dev.studica.com/maven/release/2024/" + ], + "jsonUrl": "https://dev.studica.com/releases/2024/NavX.json", + "javaDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-frc-java", + "version": "2024.1.0" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-frc-cpp", + "version": "2024.1.0", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": false, + "libName": "navx_frc", + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxraspbian", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json new file mode 100644 index 00000000..e334459f --- /dev/null +++ b/vendordeps/PathplannerLib.json @@ -0,0 +1,38 @@ +{ + "fileName": "PathplannerLib.json", + "name": "PathplannerLib", + "version": "2024.1.1", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2024", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2024.1.1" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2024.1.1", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json new file mode 100644 index 00000000..70202069 --- /dev/null +++ b/vendordeps/Phoenix6.json @@ -0,0 +1,339 @@ +{ + "fileName": "Phoenix6.json", + "name": "CTRE-Phoenix (v6)", + "version": "24.0.0-beta-8", + "frcYear": 2024, + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-beta-latest.json", + "conflictsWith": [ + { + "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", + "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", + "offlineFileName": "Phoenix6And5.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "24.0.0-beta-8" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "24.0.0-beta-8", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "24.0.0-beta-8", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "24.0.0-beta-8", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonFX", + "version": "24.0.0-beta-8", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "24.0.0-beta-8", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "24.0.0-beta-8", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "24.0.0-beta-8", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "24.0.0-beta-8", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "24.0.0-beta-8", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "24.0.0-beta-8", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "24.0.0-beta-8", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "24.0.0-beta-8", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "24.0.0-beta-8", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "24.0.0-beta-8", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "24.0.0-beta-8", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonFX", + "version": "24.0.0-beta-8", + "libName": "CTRE_SimTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "24.0.0-beta-8", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "24.0.0-beta-8", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "24.0.0-beta-8", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "24.0.0-beta-8", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "24.0.0-beta-8", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "24.0.0-beta-8", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 00000000..0f3520e7 --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,74 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2024.2.0", + "frcYear": "2024", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2024.2.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2024.2.0", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.0", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/ReduxLib_2023.json b/vendordeps/ReduxLib_2023.json new file mode 100644 index 00000000..5d3ea748 --- /dev/null +++ b/vendordeps/ReduxLib_2023.json @@ -0,0 +1,55 @@ +{ + "fileName": "ReduxLib_2023.json", + "name": "ReduxLib", + "version": "2023.2.0", + "frcYear": 2024, + "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", + "mavenUrls": [ + "https://maven.reduxrobotics.com/" + ], + "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2023.json", + "javaDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-java", + "version": "2023.2.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-driver", + "version": "2023.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-cpp", + "version": "2023.2.0", + "libName": "ReduxLib-cpp", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ] +} \ No newline at end of file From ef38a9c3c631bc72670de13d7f91cd847bd05a5b Mon Sep 17 00:00:00 2001 From: DriverStation3 Date: Tue, 9 Jan 2024 20:26:11 -0800 Subject: [PATCH 06/12] drive not working :( --- simgui-ds.json | 5 + simgui.json | 14 + .../practiceswerve/modules/backleft.json | 6 +- .../practiceswerve/modules/backright.json | 6 +- .../practiceswerve/modules/frontleft.json | 6 +- .../practiceswerve/modules/frontright.json | 6 +- .../modules/physicalproperties.json | 4 +- .../modules/pidfproperties.json | 2 +- .../robot/subsystems/DrivebaseSubsystem.java | 8 +- .../swervelib/encoders/CANCoderSwerve.java | 2 +- ...IS16470Swerve.txt => ADIS16470Swerve.java} | 2 +- src/main/java/swervelib/imu/NavXSwerve.java | 13 +- .../{PigeonSwerve.txt => PigeonSwerve.java} | 0 .../swervelib/motors/SparkFlexSwerve.java | 447 ++++++++++++++++++ ...ve.txt => SparkMaxBrushedMotorSwerve.java} | 4 +- ...SparkMaxSwerve.txt => SparkMaxSwerve.java} | 4 +- ...TalonSRXSwerve.txt => TalonSRXSwerve.java} | 0 .../swervelib/parser/json/DeviceJson.java | 38 +- vendordeps/Phoenix5.json | 151 ++++++ vendordeps/Phoenix6.json | 50 +- 20 files changed, 683 insertions(+), 85 deletions(-) rename src/main/java/swervelib/imu/{ADIS16470Swerve.txt => ADIS16470Swerve.java} (96%) rename src/main/java/swervelib/imu/{PigeonSwerve.txt => PigeonSwerve.java} (100%) create mode 100644 src/main/java/swervelib/motors/SparkFlexSwerve.java rename src/main/java/swervelib/motors/{SparkMaxBrushedMotorSwerve.txt => SparkMaxBrushedMotorSwerve.java} (99%) rename src/main/java/swervelib/motors/{SparkMaxSwerve.txt => SparkMaxSwerve.java} (99%) rename src/main/java/swervelib/motors/{TalonSRXSwerve.txt => TalonSRXSwerve.java} (100%) create mode 100644 vendordeps/Phoenix5.json diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713c..69b1a3cb 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -88,5 +88,10 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + } ] } diff --git a/simgui.json b/simgui.json index 89168d8e..35cb8934 100644 --- a/simgui.json +++ b/simgui.json @@ -1,4 +1,18 @@ { + "HALProvider": { + "Other Devices": { + "Talon FX (v6)[1]": { + "header": { + "open": true + } + }, + "Talon FX (v6)[2]": { + "header": { + "open": true + } + } + } + }, "NTProvider": { "types": { "/FMSInfo": "FMSInfo", diff --git a/src/main/deploy/practiceswerve/modules/backleft.json b/src/main/deploy/practiceswerve/modules/backleft.json index a59f11fa..ee4fda39 100644 --- a/src/main/deploy/practiceswerve/modules/backleft.json +++ b/src/main/deploy/practiceswerve/modules/backleft.json @@ -14,10 +14,6 @@ "id": 8, "canbus": null }, - "conversionFactor": { - "angle": 0.01373291, - "drive": 0.000019146492 - }, "encoder": { "type": "cancoder", "id": 9, @@ -25,6 +21,6 @@ }, "inverted": { "drive": false, - "angle": false + "angle": true } } \ No newline at end of file diff --git a/src/main/deploy/practiceswerve/modules/backright.json b/src/main/deploy/practiceswerve/modules/backright.json index abcdbcc9..464df5fd 100644 --- a/src/main/deploy/practiceswerve/modules/backright.json +++ b/src/main/deploy/practiceswerve/modules/backright.json @@ -14,10 +14,6 @@ "id": 11, "canbus": null }, - "conversionFactor": { - "angle": 0.01373291, - "drive": 0.000019146492 - }, "encoder": { "type": "cancoder", "id": 12, @@ -25,6 +21,6 @@ }, "inverted": { "drive": false, - "angle": false + "angle": true } } \ No newline at end of file diff --git a/src/main/deploy/practiceswerve/modules/frontleft.json b/src/main/deploy/practiceswerve/modules/frontleft.json index 6c00fe7c..8060e3e9 100644 --- a/src/main/deploy/practiceswerve/modules/frontleft.json +++ b/src/main/deploy/practiceswerve/modules/frontleft.json @@ -14,10 +14,6 @@ "id": 2, "canbus": null }, - "conversionFactor": { - "angle": 0.01373291, - "drive": 0.000019146492 - }, "encoder": { "type": "cancoder", "id": 3, @@ -25,6 +21,6 @@ }, "inverted": { "drive": false, - "angle": false + "angle": true } } \ No newline at end of file diff --git a/src/main/deploy/practiceswerve/modules/frontright.json b/src/main/deploy/practiceswerve/modules/frontright.json index 33abccf2..8ee0768c 100644 --- a/src/main/deploy/practiceswerve/modules/frontright.json +++ b/src/main/deploy/practiceswerve/modules/frontright.json @@ -14,10 +14,6 @@ "id": 5, "canbus": null }, - "conversionFactor": { - "angle": 0.01373291, - "drive": 0.000019146492 - }, "encoder": { "type": "cancoder", "id": 6, @@ -25,6 +21,6 @@ }, "inverted": { "drive": false, - "angle": false + "angle": true } } \ No newline at end of file diff --git a/src/main/deploy/practiceswerve/modules/physicalproperties.json b/src/main/deploy/practiceswerve/modules/physicalproperties.json index 288ab51e..b4e2d01d 100644 --- a/src/main/deploy/practiceswerve/modules/physicalproperties.json +++ b/src/main/deploy/practiceswerve/modules/physicalproperties.json @@ -6,8 +6,8 @@ "angle": 20 }, "conversionFactor": { - "angle": 0.01373291, - "drive": 0.000019146492 + "angle": 28.125, + "drive": 25.5023865506 }, "rampRate": { "drive": 0.25, diff --git a/src/main/deploy/practiceswerve/modules/pidfproperties.json b/src/main/deploy/practiceswerve/modules/pidfproperties.json index 3834a362..414995eb 100644 --- a/src/main/deploy/practiceswerve/modules/pidfproperties.json +++ b/src/main/deploy/practiceswerve/modules/pidfproperties.json @@ -7,7 +7,7 @@ "iz": 0 }, "angle": { - "p": 0.0020645, + "p": 5.0, "i": 0, "d": 0, "f": 0, diff --git a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java index 7ce7c955..0110b9fa 100644 --- a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java @@ -26,7 +26,7 @@ public class DrivebaseSubsystem extends SubsystemBase { // SWERVE CONSTANTS (that aren't in deploy dir) private static final double MAX_SPEED = 1.0; - private static final double JOYSTICK_DEADBAND = 0.05; + private static final double JOYSTICK_DEADBAND = 0.0; private static final double DRIVEBASE_RADIUS = 0; // AUTO CONSTANTS @@ -42,8 +42,10 @@ public DrivebaseSubsystem() { if (Robot.getInstance().isCompetition()) { swerveJsonDirectory = new File(Filesystem.getDeployDirectory(), "swerve"); + System.out.println("Running competition swerve"); } else { swerveJsonDirectory = new File(Filesystem.getDeployDirectory(), "practiceswerve"); + System.out.println("Running practice swerve"); } try { @@ -55,13 +57,13 @@ public DrivebaseSubsystem() { // set drive motors to brake swerveDrive.setMotorIdleMode(true); // enable optimization (never move the angle wheels more than 90 degrees) - swerveDrive.setModuleStateOptimization(true); + swerveDrive.setModuleStateOptimization(false); // swerve drive heading will slowly drift over time as you translate. this method enables an // active correction using pid. disabled until testing can be done swerveDrive.setHeadingCorrection(false); // supposed to do something? see // https://broncbotz3481.github.io/YAGSL/swervelib/SwerveDrive.html#chassisVelocityCorrection - swerveDrive.chassisVelocityCorrection = true; + swerveDrive.chassisVelocityCorrection = false; swerveDrive.synchronizeModuleEncoders(); diff --git a/src/main/java/swervelib/encoders/CANCoderSwerve.java b/src/main/java/swervelib/encoders/CANCoderSwerve.java index 5d8c3283..57ff8b9b 100644 --- a/src/main/java/swervelib/encoders/CANCoderSwerve.java +++ b/src/main/java/swervelib/encoders/CANCoderSwerve.java @@ -97,7 +97,7 @@ public double getAbsolutePosition() if (strength == MagnetHealthValue.Magnet_Invalid || strength == MagnetHealthValue.Magnet_Red) { readingError = true; - DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " reading was faulty.\n", false); + // DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " reading was faulty.\n", false); return 0; } StatusSignal angle = encoder.getAbsolutePosition().refresh(); diff --git a/src/main/java/swervelib/imu/ADIS16470Swerve.txt b/src/main/java/swervelib/imu/ADIS16470Swerve.java similarity index 96% rename from src/main/java/swervelib/imu/ADIS16470Swerve.txt rename to src/main/java/swervelib/imu/ADIS16470Swerve.java index c93dcb67..16d06fe5 100644 --- a/src/main/java/swervelib/imu/ADIS16470Swerve.txt +++ b/src/main/java/swervelib/imu/ADIS16470Swerve.java @@ -68,7 +68,7 @@ public void setOffset(Rotation3d offset) */ public Rotation3d getRawRotation3d() { - return new Rotation3d(0, 0, Math.toRadians(-imu.getAngle())); + return new Rotation3d(0, 0, Math.toRadians(-imu.getAngle(imu.getYawAxis()))); } /** diff --git a/src/main/java/swervelib/imu/NavXSwerve.java b/src/main/java/swervelib/imu/NavXSwerve.java index f793300b..272efafa 100644 --- a/src/main/java/swervelib/imu/NavXSwerve.java +++ b/src/main/java/swervelib/imu/NavXSwerve.java @@ -96,10 +96,7 @@ public NavXSwerve(I2C.Port port) public void factoryDefault() { // gyro.reset(); // Reported to be slow - offset = new Rotation3d(new Quaternion(gyro.getQuaternionW(), - gyro.getQuaternionX(), - gyro.getQuaternionY(), - gyro.getQuaternionZ())); + offset = gyro.getRotation3d(); } /** @@ -125,12 +122,10 @@ public void setOffset(Rotation3d offset) * * @return {@link Rotation3d} from the IMU. */ + @Override public Rotation3d getRawRotation3d() { - return new Rotation3d(new Quaternion(gyro.getQuaternionW(), - gyro.getQuaternionX(), - gyro.getQuaternionY(), - gyro.getQuaternionZ())); + return gyro.getRotation3d(); } /** @@ -141,7 +136,7 @@ public Rotation3d getRawRotation3d() @Override public Rotation3d getRotation3d() { - return getRawRotation3d().minus(offset); + return gyro.getRotation3d().minus(offset); } /** diff --git a/src/main/java/swervelib/imu/PigeonSwerve.txt b/src/main/java/swervelib/imu/PigeonSwerve.java similarity index 100% rename from src/main/java/swervelib/imu/PigeonSwerve.txt rename to src/main/java/swervelib/imu/PigeonSwerve.java diff --git a/src/main/java/swervelib/motors/SparkFlexSwerve.java b/src/main/java/swervelib/motors/SparkFlexSwerve.java new file mode 100644 index 00000000..a5aef968 --- /dev/null +++ b/src/main/java/swervelib/motors/SparkFlexSwerve.java @@ -0,0 +1,447 @@ +package swervelib.motors; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.CANSparkFlex; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkLowLevel.PeriodicFrame; +import com.revrobotics.MotorFeedbackSensor; +import com.revrobotics.REVLibError; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkAnalogSensor; +import com.revrobotics.SparkPIDController; +import edu.wpi.first.wpilibj.DriverStation; +import java.util.function.Supplier; +import swervelib.encoders.SwerveAbsoluteEncoder; +import swervelib.parser.PIDFConfig; +import swervelib.telemetry.SwerveDriveTelemetry; + +/** + * An implementation of {@link CANSparkFlex} as a {@link SwerveMotor}. + */ +public class SparkFlexSwerve extends SwerveMotor +{ + + /** + * SparkMAX Instance. + */ + public CANSparkFlex motor; + /** + * Integrated encoder. + */ + public RelativeEncoder encoder; + /** + * Absolute encoder attached to the SparkMax (if exists) + */ + public SwerveAbsoluteEncoder absoluteEncoder; + /** + * Closed-loop PID controller. + */ + public SparkPIDController pid; + /** + * Factory default already occurred. + */ + private boolean factoryDefaultOccurred = false; + + /** + * Initialize the swerve motor. + * + * @param motor The SwerveMotor as a SparkFlex object. + * @param isDriveMotor Is the motor being initialized a drive motor? + */ + public SparkFlexSwerve(CANSparkFlex motor, boolean isDriveMotor) + { + this.motor = motor; + this.isDriveMotor = isDriveMotor; + factoryDefaults(); + clearStickyFaults(); + + encoder = motor.getEncoder(); + pid = motor.getPIDController(); + pid.setFeedbackDevice( + encoder); // Configure feedback of the PID controller as the integrated encoder. + + // Spin off configurations in a different thread. + // configureSparkMax(() -> motor.setCANTimeout(0)); // Commented out because it prevents feedback. + } + + /** + * Initialize the {@link SwerveMotor} as a {@link CANSparkMax} connected to a Brushless Motor. + * + * @param id CAN ID of the SparkMax. + * @param isDriveMotor Is the motor being initialized a drive motor? + */ + public SparkFlexSwerve(int id, boolean isDriveMotor) + { + this(new CANSparkFlex(id, MotorType.kBrushless), isDriveMotor); + } + + /** + * Run the configuration until it succeeds or times out. + * + * @param config Lambda supplier returning the error state. + */ + private void configureSparkFlex(Supplier config) + { + for (int i = 0; i < maximumRetries; i++) + { + if (config.get() == REVLibError.kOk) + { + return; + } + } + DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true); + } + + /** + * Set the voltage compensation for the swerve module motor. + * + * @param nominalVoltage Nominal voltage for operation to output to. + */ + @Override + public void setVoltageCompensation(double nominalVoltage) + { + configureSparkFlex(() -> motor.enableVoltageCompensation(nominalVoltage)); + } + + /** + * Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with + * voltage compensation. This is useful to protect the motor from current spikes. + * + * @param currentLimit Current limit in AMPS at free speed. + */ + @Override + public void setCurrentLimit(int currentLimit) + { + configureSparkFlex(() -> motor.setSmartCurrentLimit(currentLimit)); + } + + /** + * Set the maximum rate the open/closed loop output can change by. + * + * @param rampRate Time in seconds to go from 0 to full throttle. + */ + @Override + public void setLoopRampRate(double rampRate) + { + configureSparkFlex(() -> motor.setOpenLoopRampRate(rampRate)); + configureSparkFlex(() -> motor.setClosedLoopRampRate(rampRate)); + } + + /** + * Get the motor object from the module. + * + * @return Motor object. + */ + @Override + public Object getMotor() + { + return motor; + } + + /** + * Queries whether the absolute encoder is directly attached to the motor controller. + * + * @return connected absolute encoder state. + */ + @Override + public boolean isAttachedAbsoluteEncoder() + { + return absoluteEncoder != null; + } + + /** + * Configure the factory defaults. + */ + @Override + public void factoryDefaults() + { + if (!factoryDefaultOccurred) + { + configureSparkFlex(motor::restoreFactoryDefaults); + factoryDefaultOccurred = true; + } + } + + /** + * Clear the sticky faults on the motor controller. + */ + @Override + public void clearStickyFaults() + { + configureSparkFlex(motor::clearFaults); + } + + /** + * Set the absolute encoder to be a compatible absolute encoder. + * + * @param encoder The encoder to use. + * @return The {@link SwerveMotor} for easy instantiation. + */ + @Override + public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) + { + if (encoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) + { + DriverStation.reportWarning( + "IF possible configure the duty cycle encoder offset in the REV Hardware Client instead of using the" + + " absoluteEncoderOffset in the Swerve Module JSON!", + false); + absoluteEncoder = encoder; + configureSparkFlex(() -> pid.setFeedbackDevice((MotorFeedbackSensor) absoluteEncoder.getAbsoluteEncoder())); + } + return this; + } + + /** + * Configure the integrated encoder for the swerve module. Sets the conversion factors for position and velocity. + * + * @param positionConversionFactor The conversion factor to apply. + */ + @Override + public void configureIntegratedEncoder(double positionConversionFactor) + { + if (absoluteEncoder == null) + { + configureSparkFlex(() -> encoder.setPositionConversionFactor(positionConversionFactor)); + configureSparkFlex(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60)); + + // Taken from + // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67 + configureCANStatusFrames(10, 20, 20, 500, 500); + } else + { + configureSparkFlex(() -> { + if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) + { + return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor( + positionConversionFactor); + } else + { + return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor( + positionConversionFactor); + } + }); + configureSparkFlex(() -> { + if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) + { + return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor( + positionConversionFactor / 60); + } else + { + return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor( + positionConversionFactor / 60); + } + }); + } + } + + /** + * Configure the PIDF values for the closed loop controller. + * + * @param config Configuration class holding the PIDF values. + */ + @Override + public void configurePIDF(PIDFConfig config) + { +// int pidSlot = +// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal(); + int pidSlot = 0; + configureSparkFlex(() -> pid.setP(config.p, pidSlot)); + configureSparkFlex(() -> pid.setI(config.i, pidSlot)); + configureSparkFlex(() -> pid.setD(config.d, pidSlot)); + configureSparkFlex(() -> pid.setFF(config.f, pidSlot)); + configureSparkFlex(() -> pid.setIZone(config.iz, pidSlot)); + configureSparkFlex(() -> pid.setOutputRange(config.output.min, config.output.max, pidSlot)); + } + + /** + * Configure the PID wrapping for the position closed loop controller. + * + * @param minInput Minimum PID input. + * @param maxInput Maximum PID input. + */ + @Override + public void configurePIDWrapping(double minInput, double maxInput) + { + configureSparkFlex(() -> pid.setPositionPIDWrappingEnabled(true)); + configureSparkFlex(() -> pid.setPositionPIDWrappingMinInput(minInput)); + configureSparkFlex(() -> pid.setPositionPIDWrappingMaxInput(maxInput)); + } + + /** + * Set the CAN status frames. + * + * @param CANStatus0 Applied Output, Faults, Sticky Faults, Is Follower + * @param CANStatus1 Motor Velocity, Motor Temperature, Motor Voltage, Motor Current + * @param CANStatus2 Motor Position + * @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position + * @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position + */ + public void configureCANStatusFrames( + int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4) + { + configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0)); + configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1)); + configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2)); + configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3)); + configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4)); + // TODO: Configure Status Frame 5 and 6 if necessary + // https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces + } + + /** + * Set the idle mode. + * + * @param isBrakeMode Set the brake mode. + */ + @Override + public void setMotorBrake(boolean isBrakeMode) + { + configureSparkFlex(() -> motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast)); + } + + /** + * Set the motor to be inverted. + * + * @param inverted State of inversion. + */ + @Override + public void setInverted(boolean inverted) + { + motor.setInverted(inverted); + } + + /** + * Save the configurations from flash to EEPROM. + */ + @Override + public void burnFlash() + { + try + { + Thread.sleep(200); + } catch (Exception e) + { + } + configureSparkFlex(() -> motor.burnFlash()); + } + + /** + * Set the percentage output. + * + * @param percentOutput percent out for the motor controller. + */ + @Override + public void set(double percentOutput) + { + motor.set(percentOutput); + } + + /** + * Set the closed loop PID controller reference point. + * + * @param setpoint Setpoint in MPS or Angle in degrees. + * @param feedforward Feedforward in volt-meter-per-second or kV. + */ + @Override + public void setReference(double setpoint, double feedforward) + { + boolean possibleBurnOutIssue = true; +// int pidSlot = +// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal(); + int pidSlot = 0; + + if (isDriveMotor) + { + configureSparkFlex(() -> + pid.setReference( + setpoint, + ControlType.kVelocity, + pidSlot, + feedforward)); + } else + { + configureSparkFlex(() -> + pid.setReference( + setpoint, + ControlType.kPosition, + pidSlot, + feedforward)); + if(SwerveDriveTelemetry.isSimulation) + { + encoder.setPosition(setpoint); + } + } + } + + /** + * Set the closed loop PID controller reference point. + * + * @param setpoint Setpoint in meters per second or angle in degrees. + * @param feedforward Feedforward in volt-meter-per-second or kV. + * @param position Only used on the angle motor, the position of the motor in degrees. + */ + @Override + public void setReference(double setpoint, double feedforward, double position) + { + setReference(setpoint, feedforward); + } + + /** + * Get the velocity of the integrated encoder. + * + * @return velocity + */ + @Override + public double getVelocity() + { + return absoluteEncoder == null ? encoder.getVelocity() : absoluteEncoder.getVelocity(); + } + + /** + * Get the position of the integrated encoder. + * + * @return Position + */ + @Override + public double getPosition() + { + return absoluteEncoder == null ? encoder.getPosition() : absoluteEncoder.getAbsolutePosition(); + } + + /** + * Set the integrated encoder position. + * + * @param position Integrated encoder position. + */ + @Override + public void setPosition(double position) + { + if (absoluteEncoder == null) + { + configureSparkFlex(() -> encoder.setPosition(position)); + } + } + + /** + * REV Slots for PID configuration. + */ + enum SparkMAX_slotIdx + { + /** + * Slot 1, used for position PID's. + */ + Position, + /** + * Slot 2, used for velocity PID's. + */ + Velocity, + /** + * Slot 3, used arbitrarily. (Documentation show simulations). + */ + Simulation + } +} diff --git a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.txt b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java similarity index 99% rename from src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.txt rename to src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java index ba1add7a..fb12b181 100644 --- a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.txt +++ b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -2,8 +2,8 @@ import com.revrobotics.AbsoluteEncoder; import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMax.ControlType; -import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame; import com.revrobotics.REVLibError; diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.txt b/src/main/java/swervelib/motors/SparkMaxSwerve.java similarity index 99% rename from src/main/java/swervelib/motors/SparkMaxSwerve.txt rename to src/main/java/swervelib/motors/SparkMaxSwerve.java index 7ec5c2b8..ba6f0450 100644 --- a/src/main/java/swervelib/motors/SparkMaxSwerve.txt +++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java @@ -2,8 +2,8 @@ import com.revrobotics.AbsoluteEncoder; import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMax.ControlType; -import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame; import com.revrobotics.MotorFeedbackSensor; diff --git a/src/main/java/swervelib/motors/TalonSRXSwerve.txt b/src/main/java/swervelib/motors/TalonSRXSwerve.java similarity index 100% rename from src/main/java/swervelib/motors/TalonSRXSwerve.txt rename to src/main/java/swervelib/motors/TalonSRXSwerve.java diff --git a/src/main/java/swervelib/parser/json/DeviceJson.java b/src/main/java/swervelib/parser/json/DeviceJson.java index ab5d86fb..8973478f 100644 --- a/src/main/java/swervelib/parser/json/DeviceJson.java +++ b/src/main/java/swervelib/parser/json/DeviceJson.java @@ -13,18 +13,19 @@ import swervelib.encoders.SparkMaxEncoderSwerve; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.imu.ADIS16448Swerve; -// import swervelib.imu.ADIS16470Swerve; +import swervelib.imu.ADIS16470Swerve; import swervelib.imu.ADXRS450Swerve; import swervelib.imu.AnalogGyroSwerve; import swervelib.imu.NavXSwerve; import swervelib.imu.Pigeon2Swerve; -// import swervelib.imu.PigeonSwerve; +import swervelib.imu.PigeonSwerve; import swervelib.imu.SwerveIMU; -// import swervelib.motors.SparkMaxBrushedMotorSwerve; -// import swervelib.motors.SparkMaxSwerve; +import swervelib.motors.SparkFlexSwerve; +import swervelib.motors.SparkMaxBrushedMotorSwerve; +import swervelib.motors.SparkMaxSwerve; import swervelib.motors.SwerveMotor; import swervelib.motors.TalonFXSwerve; -// import swervelib.motors.TalonSRXSwerve; +import swervelib.motors.TalonSRXSwerve; /** * Device JSON parsed class. Used to access the JSON data. @@ -106,8 +107,7 @@ public SwerveIMU createIMU() case "adis16448": return new ADIS16448Swerve(); case "adis16470": - // return new ADIS16470Swerve(); - return null; + return new ADIS16470Swerve(); case "adxrs450": return new ADXRS450Swerve(); case "analog": @@ -127,8 +127,7 @@ public SwerveIMU createIMU() case "navx_mxp": return new NavXSwerve(Port.kMXP); case "pigeon": - // return new PigeonSwerve(id); - return null; + return new PigeonSwerve(id); case "pigeon2": return new Pigeon2Swerve(id, canbus != null ? canbus : ""); default: @@ -155,34 +154,35 @@ public SwerveMotor createMotor(boolean isDriveMotor) switch (canbus) { case "greyhill_63r256": - // return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 1024, false); + return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 1024, false); case "srx_mag_encoder": - // return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 4096, false); + return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 4096, false); case "throughbore": - // return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 8192, false); + return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 8192, false); case "throughbore_dataport": - // return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kNoSensor, 8192, true); + return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kNoSensor, 8192, true); case "greyhill_63r256_dataport": - // return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 1024, true); + return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 1024, true); case "srx_mag_encoder_dataport": - // return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 4096, true); + return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 4096, true); default: if (isDriveMotor) { throw new RuntimeException("Spark MAX " + id + " MUST have a encoder attached to the motor controller."); } // We are creating a motor for an angle motor which will use the absolute encoder attached to the data port. - // return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kNoSensor, 0, false); - return null; + return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kNoSensor, 0, false); } case "neo": case "sparkmax": - // return new SparkMaxSwerve(id, isDriveMotor); + return new SparkMaxSwerve(id, isDriveMotor); + case "sparkflex": + return new SparkFlexSwerve(id, isDriveMotor); case "falcon": case "talonfx": return new TalonFXSwerve(id, canbus != null ? canbus : "", isDriveMotor); case "talonsrx": - // return new TalonSRXSwerve(id, isDriveMotor); + return new TalonSRXSwerve(id, isDriveMotor); default: throw new RuntimeException(type + " is not a recognized motor type."); } diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json new file mode 100644 index 00000000..88a68dd0 --- /dev/null +++ b/vendordeps/Phoenix5.json @@ -0,0 +1,151 @@ +{ + "fileName": "Phoenix5.json", + "name": "CTRE-Phoenix (v5)", + "version": "5.33.0", + "frcYear": 2024, + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json", + "requires": [ + { + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", + "offlineFileName": "Phoenix6.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.33.0" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.33.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.33.0", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.33.0", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.0", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.33.0", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.33.0", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.0", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json index 70202069..69a40798 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6.json @@ -1,13 +1,13 @@ { "fileName": "Phoenix6.json", "name": "CTRE-Phoenix (v6)", - "version": "24.0.0-beta-8", + "version": "24.1.0", "frcYear": 2024, "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-beta-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json", "conflictsWith": [ { "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.0.0-beta-8" + "version": "24.1.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.0.0-beta-8", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -39,7 +39,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.0.0-beta-8", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -52,7 +52,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.0.0-beta-8", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -65,7 +65,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.0.0-beta-8", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -78,7 +78,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.0.0-beta-8", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -91,7 +91,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.0.0-beta-8", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -104,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.0.0-beta-8", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -117,7 +117,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.0.0-beta-8", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -130,7 +130,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.0.0-beta-8", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -143,7 +143,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.0.0-beta-8", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -158,7 +158,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.0.0-beta-8", + "version": "24.1.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -173,7 +173,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.0.0-beta-8", + "version": "24.1.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -188,7 +188,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.0.0-beta-8", + "version": "24.1.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -203,7 +203,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.0.0-beta-8", + "version": "24.1.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -218,7 +218,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.0.0-beta-8", + "version": "24.1.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -233,7 +233,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.0.0-beta-8", + "version": "24.1.0", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -248,7 +248,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.0.0-beta-8", + "version": "24.1.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -263,7 +263,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.0.0-beta-8", + "version": "24.1.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -278,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.0.0-beta-8", + "version": "24.1.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -293,7 +293,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.0.0-beta-8", + "version": "24.1.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.0.0-beta-8", + "version": "24.1.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -323,7 +323,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.0.0-beta-8", + "version": "24.1.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, From b115a2aea97185d3c4289d4585165b325929b266 Mon Sep 17 00:00:00 2001 From: DriverStation3 Date: Fri, 12 Jan 2024 20:23:39 -0800 Subject: [PATCH 07/12] it works! --- .../practiceswerve/modules/backleft.json | 4 +- .../practiceswerve/modules/backright.json | 4 +- .../practiceswerve/modules/frontleft.json | 4 +- .../practiceswerve/modules/frontright.json | 4 +- .../modules/physicalproperties.json | 4 +- .../modules/pidfproperties.json | 4 +- .../deploy/practiceswerve/swervedrive.json | 2 +- .../robot/subsystems/DrivebaseSubsystem.java | 9 +- src/main/java/swervelib/SwerveDrive.java | 23 +---- src/main/java/swervelib/SwerveModule.java | 91 +++++++++++-------- .../swervelib/encoders/CANCoderSwerve.java | 2 +- .../encoders/SparkMaxAnalogEncoderSwerve.java | 8 +- .../encoders/SparkMaxEncoderSwerve.java | 2 +- .../java/swervelib/imu/ADIS16470Swerve.java | 3 +- src/main/java/swervelib/math/SwerveMath.java | 16 +--- .../motors/SparkMaxBrushedMotorSwerve.java | 10 +- .../java/swervelib/motors/SparkMaxSwerve.java | 19 ++-- .../java/swervelib/motors/TalonFXSwerve.java | 16 ++-- .../swervelib/parser/json/DeviceJson.java | 2 +- 19 files changed, 110 insertions(+), 117 deletions(-) diff --git a/src/main/deploy/practiceswerve/modules/backleft.json b/src/main/deploy/practiceswerve/modules/backleft.json index ee4fda39..10e055b1 100644 --- a/src/main/deploy/practiceswerve/modules/backleft.json +++ b/src/main/deploy/practiceswerve/modules/backleft.json @@ -20,7 +20,7 @@ "canbus": null }, "inverted": { - "drive": false, - "angle": true + "drive": true, + "angle": false } } \ No newline at end of file diff --git a/src/main/deploy/practiceswerve/modules/backright.json b/src/main/deploy/practiceswerve/modules/backright.json index 464df5fd..731ac08a 100644 --- a/src/main/deploy/practiceswerve/modules/backright.json +++ b/src/main/deploy/practiceswerve/modules/backright.json @@ -20,7 +20,7 @@ "canbus": null }, "inverted": { - "drive": false, - "angle": true + "drive": true, + "angle": false } } \ No newline at end of file diff --git a/src/main/deploy/practiceswerve/modules/frontleft.json b/src/main/deploy/practiceswerve/modules/frontleft.json index 8060e3e9..81199fee 100644 --- a/src/main/deploy/practiceswerve/modules/frontleft.json +++ b/src/main/deploy/practiceswerve/modules/frontleft.json @@ -20,7 +20,7 @@ "canbus": null }, "inverted": { - "drive": false, - "angle": true + "drive": true, + "angle": false } } \ No newline at end of file diff --git a/src/main/deploy/practiceswerve/modules/frontright.json b/src/main/deploy/practiceswerve/modules/frontright.json index 8ee0768c..64bcbfaa 100644 --- a/src/main/deploy/practiceswerve/modules/frontright.json +++ b/src/main/deploy/practiceswerve/modules/frontright.json @@ -20,7 +20,7 @@ "canbus": null }, "inverted": { - "drive": false, - "angle": true + "drive": true, + "angle": false } } \ No newline at end of file diff --git a/src/main/deploy/practiceswerve/modules/physicalproperties.json b/src/main/deploy/practiceswerve/modules/physicalproperties.json index b4e2d01d..dbeccd81 100644 --- a/src/main/deploy/practiceswerve/modules/physicalproperties.json +++ b/src/main/deploy/practiceswerve/modules/physicalproperties.json @@ -6,8 +6,8 @@ "angle": 20 }, "conversionFactor": { - "angle": 28.125, - "drive": 25.5023865506 + "angle": 12.8, + "drive": 25.50238655060027 }, "rampRate": { "drive": 0.25, diff --git a/src/main/deploy/practiceswerve/modules/pidfproperties.json b/src/main/deploy/practiceswerve/modules/pidfproperties.json index 414995eb..90d78d79 100644 --- a/src/main/deploy/practiceswerve/modules/pidfproperties.json +++ b/src/main/deploy/practiceswerve/modules/pidfproperties.json @@ -1,13 +1,13 @@ { "drive": { - "p": 0.0020645, + "p": 2.0, "i": 0, "d": 0, "f": 0, "iz": 0 }, "angle": { - "p": 5.0, + "p": 30.0, "i": 0, "d": 0, "f": 0, diff --git a/src/main/deploy/practiceswerve/swervedrive.json b/src/main/deploy/practiceswerve/swervedrive.json index d2b6dbd5..0409e297 100644 --- a/src/main/deploy/practiceswerve/swervedrive.json +++ b/src/main/deploy/practiceswerve/swervedrive.json @@ -4,7 +4,7 @@ "id": 0, "canbus": null }, - "invertedIMU": true, + "invertedIMU": false, "modules": [ "frontleft.json", "frontright.json", diff --git a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java index 0110b9fa..67dc73bf 100644 --- a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java @@ -20,12 +20,13 @@ import swervelib.SwerveDrive; import swervelib.math.SwerveMath; import swervelib.parser.SwerveParser; +import swervelib.telemetry.SwerveDriveTelemetry; public class DrivebaseSubsystem extends SubsystemBase { // SWERVE CONSTANTS (that aren't in deploy dir) - private static final double MAX_SPEED = 1.0; + private static final double MAX_SPEED = 0.1; private static final double JOYSTICK_DEADBAND = 0.0; private static final double DRIVEBASE_RADIUS = 0; @@ -57,7 +58,7 @@ public DrivebaseSubsystem() { // set drive motors to brake swerveDrive.setMotorIdleMode(true); // enable optimization (never move the angle wheels more than 90 degrees) - swerveDrive.setModuleStateOptimization(false); + // swerveDrive.setModuleStateOptimization(false); // swerve drive heading will slowly drift over time as you translate. this method enables an // active correction using pid. disabled until testing can be done swerveDrive.setHeadingCorrection(false); @@ -81,6 +82,8 @@ public DrivebaseSubsystem() { new ReplanningConfig()), ()->DriverStation.getAlliance().get().equals(Alliance.Red), // flip path if on the red alliance this); + + SwerveDriveTelemetry.verbosity = SwerveDriveTelemetry.TelemetryVerbosity.HIGH; } /** @@ -148,7 +151,7 @@ public void resetGyro() { /** Reset everything we can on the drivebase. To be used before auto starts */ public void resetRobot() { - swerveDrive.resetEncoders(); + swerveDrive.resetDriveEncoders(); resetGyro(); setPose(new Pose2d()); } diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index ec7f452d..0f727b0a 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -881,13 +881,7 @@ public void updateOdometry() sumVelocity += Math.abs(moduleState.speedMetersPerSecond); if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) { - SmartDashboard.putNumber( - "Module[" + module.configuration.name + "] Relative Encoder", module.getRelativePosition()); - SmartDashboard.putNumber( - "Module[" + module.configuration.name + "] Absolute Encoder", module.getAbsolutePosition()); - SmartDashboard.putNumber( - "Module[" + module.configuration.name + "] Absolute Encoder Read Issue", - module.getAbsoluteEncoderReadIssue() ? 1 : 0); + module.updateTelemetry(); } if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) { @@ -1035,7 +1029,7 @@ public SwerveModule[] getModules() * Reset the drive encoders on the robot, useful when manually resetting the robot without a reboot, like in * autonomous. */ - public void resetEncoders() + public void resetDriveEncoders() { for (SwerveModule module : swerveModules) { @@ -1043,19 +1037,6 @@ public void resetEncoders() } } - /** - * Configure whether the {@link SwerveModuleState} will be optimized to prevent spinning more than 90deg. - * - * @param optimizationEnabled Whether the module optimization is enabled. - */ - public void setModuleStateOptimization(boolean optimizationEnabled) - { - for (SwerveModule module : swerveModules) - { - module.moduleStateOptimization = optimizationEnabled; - } - } - /** * Pushes the Absolute Encoder offsets to the Encoder or Motor Controller, depending on type. Also removes the * internal offsets to prevent double offsetting. diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 999ccc16..6853a2ca 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -4,6 +4,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import swervelib.encoders.SwerveAbsoluteEncoder; @@ -48,15 +49,10 @@ public class SwerveModule * Last swerve module state applied. */ public SwerveModuleState lastState; - /** - * Enable {@link SwerveModuleState} optimizations so the angle is reversed and speed is reversed to ensure the module - * never turns more than 90deg. - */ - public boolean moduleStateOptimization = true; /** * Angle offset from the absolute encoder. */ - private double angleOffset; + private double angleOffset; /** * Simulated swerve module. */ @@ -114,7 +110,7 @@ public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfigurat // Config angle motor/controller angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle); angleMotor.configurePIDF(moduleConfiguration.anglePIDF); - angleMotor.configurePIDWrapping(-180, 180); + angleMotor.configurePIDWrapping(0, 90); angleMotor.setInverted(moduleConfiguration.angleMotorInverted); angleMotor.setMotorBrake(false); @@ -178,11 +174,7 @@ public void queueSynchronizeEncoders() */ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, boolean force) { - if (moduleStateOptimization) - { - desiredState = SwerveModuleState.optimize(desiredState, - Rotation2d.fromDegrees(getAbsolutePosition())); - } + desiredState = SwerveModuleState.optimize(desiredState, lastState.angle); if (isOpenLoop) { @@ -190,40 +182,44 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, driveMotor.set(percentOutput); } else { - if (desiredState.speedMetersPerSecond != lastState.speedMetersPerSecond) + // Taken from the CTRE SwerveModule class. + // https://api.ctr-electronics.com/phoenix6/release/java/src-html/com/ctre/phoenix6/mechanisms/swerve/SwerveModule.html#line.46 + /* From FRC 900's whitepaper, we add a cosine compensator to the applied drive velocity */ + /* To reduce the "skew" that occurs when changing direction */ + double steerMotorError = desiredState.angle.getDegrees() - getAbsolutePosition(); + /* If error is close to 0 rotations, we're already there, so apply full power */ + /* If the error is close to 0.25 rotations, then we're 90 degrees, so movement doesn't help us at all */ + double cosineScalar = Math.cos(Units.rotationsToRadians(steerMotorError)); + /* Make sure we don't invert our drive, even though we shouldn't ever target over 90 degrees anyway */ + if (cosineScalar < 0.0) { - double velocity = desiredState.speedMetersPerSecond; - driveMotor.setReference(velocity, feedforward.calculate(velocity)); + cosineScalar = 0.0; } + + double velocity = desiredState.speedMetersPerSecond;// * (cosineScalar); + driveMotor.setReference(velocity, 0); } + /* // Not necessary anymore. // If we are forcing the angle if (!force) { - // Prevents module rotation if speed is less than 1% + // Prevents module rotation if speed is less than 1% SwerveMath.antiJitter(desiredState, lastState, Math.min(maxSpeed, 4)); } - - if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) - { - SmartDashboard.putNumber("Module[" + configuration.name + "] Speed Setpoint:", desiredState.speedMetersPerSecond); - SmartDashboard.putNumber("Module[" + configuration.name + "] Angle Setpoint:", desiredState.angle.getDegrees()); - } + */ // Prevent module rotation if angle is the same as the previous angle. - if (desiredState.angle != lastState.angle || synchronizeEncoderQueued) + // Synchronize encoders if queued and send in the current position as the value from the absolute encoder. + if (absoluteEncoder != null && synchronizeEncoderQueued) { - // Synchronize encoders if queued and send in the current position as the value from the absolute encoder. - if (absoluteEncoder != null && synchronizeEncoderQueued) - { - double absoluteEncoderPosition = getAbsolutePosition(); - angleMotor.setPosition(absoluteEncoderPosition); - angleMotor.setReference(desiredState.angle.getDegrees(), 0, absoluteEncoderPosition); - synchronizeEncoderQueued = false; - } else - { - angleMotor.setReference(desiredState.angle.getDegrees(), 0); - } + double absoluteEncoderPosition = getAbsolutePosition(); + angleMotor.setPosition(absoluteEncoderPosition); + angleMotor.setReference(desiredState.angle.getDegrees(), 0, absoluteEncoderPosition); + synchronizeEncoderQueued = false; + } else + { + angleMotor.setReference(desiredState.angle.getDegrees(), 0); } lastState = desiredState; @@ -232,6 +228,12 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, { simModule.updateStateAndPosition(desiredState); } + + if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) + { + SmartDashboard.putNumber("Module[" + configuration.name + "] Speed Setpoint", desiredState.speedMetersPerSecond); + SmartDashboard.putNumber("Module[" + configuration.name + "] Angle Setpoint", desiredState.angle.getDegrees()); + } } /** @@ -282,10 +284,6 @@ public SwerveModulePosition getPosition() { return simModule.getPosition(); } - if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) - { - SmartDashboard.putNumber("Module[" + configuration.name + "] Angle", azimuth.getDegrees()); - } return new SwerveModulePosition(position, azimuth); } @@ -324,7 +322,7 @@ public double getAbsolutePosition() */ public double getRelativePosition() { - return angleMotor.getPosition(); + return angleMotor.getPosition() * 360; } /** @@ -434,4 +432,19 @@ public boolean getAbsoluteEncoderReadIssue() return absoluteEncoder.readingError; } } + + /** + * Update data sent to {@link SmartDashboard}. + */ + public void updateTelemetry() + { + if (absoluteEncoder != null) + { + SmartDashboard.putNumber("Module[" + configuration.name + "] Raw Absolute Encoder", + absoluteEncoder.getAbsolutePosition()); + } + SmartDashboard.putNumber("Module[" + configuration.name + "] Adjusted Absolute Encoder", getAbsolutePosition()); + SmartDashboard.putNumber("Module[" + configuration.name + "] Absolute Encoder Read Issue", + getAbsoluteEncoderReadIssue() ? 1 : 0); + } } diff --git a/src/main/java/swervelib/encoders/CANCoderSwerve.java b/src/main/java/swervelib/encoders/CANCoderSwerve.java index 57ff8b9b..5d8c3283 100644 --- a/src/main/java/swervelib/encoders/CANCoderSwerve.java +++ b/src/main/java/swervelib/encoders/CANCoderSwerve.java @@ -97,7 +97,7 @@ public double getAbsolutePosition() if (strength == MagnetHealthValue.Magnet_Invalid || strength == MagnetHealthValue.Magnet_Red) { readingError = true; - // DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " reading was faulty.\n", false); + DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " reading was faulty.\n", false); return 0; } StatusSignal angle = encoder.getAbsolutePosition().refresh(); diff --git a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java index 1f79cbf4..0508bca8 100644 --- a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java @@ -2,8 +2,8 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.REVLibError; -import com.revrobotics.SparkMaxAnalogSensor; -import com.revrobotics.SparkMaxAnalogSensor.Mode; +import com.revrobotics.SparkAnalogSensor; +import com.revrobotics.SparkAnalogSensor.Mode; import edu.wpi.first.wpilibj.DriverStation; import java.util.function.Supplier; import swervelib.motors.SwerveMotor; @@ -15,9 +15,9 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder { /** - * The {@link SparkMaxAnalogSensor} representing the duty cycle encoder attached to the SparkMax analog port. + * The {@link SparkAnalogSensor} representing the duty cycle encoder attached to the SparkMax analog port. */ - public SparkMaxAnalogSensor encoder; + public SparkAnalogSensor encoder; /** * Create the {@link SparkMaxAnalogEncoderSwerve} object as a analog sensor from the {@link CANSparkMax} motor data diff --git a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java index 87f97822..f95d36d2 100644 --- a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java @@ -3,7 +3,7 @@ import com.revrobotics.AbsoluteEncoder; import com.revrobotics.CANSparkMax; import com.revrobotics.REVLibError; -import com.revrobotics.SparkMaxAbsoluteEncoder.Type; +import com.revrobotics.SparkAbsoluteEncoder.Type; import edu.wpi.first.wpilibj.DriverStation; import java.util.function.Supplier; import swervelib.motors.SwerveMotor; diff --git a/src/main/java/swervelib/imu/ADIS16470Swerve.java b/src/main/java/swervelib/imu/ADIS16470Swerve.java index 16d06fe5..5a102bf0 100644 --- a/src/main/java/swervelib/imu/ADIS16470Swerve.java +++ b/src/main/java/swervelib/imu/ADIS16470Swerve.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis; import edu.wpi.first.wpilibj.ADIS16470_IMU; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.Optional; @@ -68,7 +69,7 @@ public void setOffset(Rotation3d offset) */ public Rotation3d getRawRotation3d() { - return new Rotation3d(0, 0, Math.toRadians(-imu.getAngle(imu.getYawAxis()))); + return new Rotation3d(0, 0, Math.toRadians(-imu.getAngle(IMUAxis.kYaw))); } /** diff --git a/src/main/java/swervelib/math/SwerveMath.java b/src/main/java/swervelib/math/SwerveMath.java index 4ab9a2de..0b4fdd5e 100644 --- a/src/main/java/swervelib/math/SwerveMath.java +++ b/src/main/java/swervelib/math/SwerveMath.java @@ -8,7 +8,6 @@ import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.List; import swervelib.SwerveController; import swervelib.SwerveModule; @@ -214,12 +213,7 @@ private static double calcMaxAccel( } double horizontalDistance = projectedHorizontalCg.plus(projectedWheelbaseEdge).getNorm(); - double maxAccel = 9.81 * horizontalDistance / robotCG.getZ(); - if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) - { - SmartDashboard.putNumber("calcMaxAccel", maxAccel); - } - return maxAccel; + return 9.81 * horizontalDistance / robotCG.getZ(); } /** @@ -275,18 +269,10 @@ public static Translation2d limitVelocity( { // Get the robot's current field-relative velocity Translation2d currentVelocity = SwerveController.getTranslation2d(fieldVelocity); - if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) - { - SmartDashboard.putNumber("currentVelocity", currentVelocity.getX()); - } // Calculate the commanded change in velocity by subtracting current velocity // from commanded velocity Translation2d deltaV = commandedVelocity.minus(currentVelocity); - if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) - { - SmartDashboard.putNumber("deltaV", deltaV.getX()); - } // Creates an acceleration vector with the direction of delta V and a magnitude // of the maximum allowed acceleration in that direction diff --git a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java index fb12b181..fc7d94ed 100644 --- a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -4,13 +4,13 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkLowLevel.PeriodicFrame; import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxAlternateEncoder; -import com.revrobotics.SparkMaxPIDController; -import com.revrobotics.SparkMaxRelativeEncoder.Type; +import com.revrobotics.SparkPIDController; +import com.revrobotics.SparkRelativeEncoder.Type; import edu.wpi.first.wpilibj.DriverStation; import java.util.function.Supplier; import swervelib.encoders.SwerveAbsoluteEncoder; @@ -38,7 +38,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor /** * Closed-loop PID controller. */ - public SparkMaxPIDController pid; + public SparkPIDController pid; /** * Factory default already occurred. */ diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java index ba6f0450..50b10ae7 100644 --- a/src/main/java/swervelib/motors/SparkMaxSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java @@ -4,17 +4,18 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkLowLevel.PeriodicFrame; import com.revrobotics.MotorFeedbackSensor; import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkMaxAnalogSensor; -import com.revrobotics.SparkMaxPIDController; +import com.revrobotics.SparkAnalogSensor; +import com.revrobotics.SparkPIDController; import edu.wpi.first.wpilibj.DriverStation; import java.util.function.Supplier; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; +import swervelib.telemetry.SwerveDriveTelemetry; /** * An implementation of {@link CANSparkMax} as a {@link SwerveMotor}. @@ -37,7 +38,7 @@ public class SparkMaxSwerve extends SwerveMotor /** * Closed-loop PID controller. */ - public SparkMaxPIDController pid; + public SparkPIDController pid; /** * Factory default already occurred. */ @@ -218,7 +219,7 @@ public void configureIntegratedEncoder(double positionConversionFactor) positionConversionFactor); } else { - return ((SparkMaxAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor( + return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor( positionConversionFactor); } }); @@ -229,7 +230,7 @@ public void configureIntegratedEncoder(double positionConversionFactor) positionConversionFactor / 60); } else { - return ((SparkMaxAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor( + return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor( positionConversionFactor / 60); } }); @@ -368,6 +369,10 @@ public void setReference(double setpoint, double feedforward) ControlType.kPosition, pidSlot, feedforward)); + if(SwerveDriveTelemetry.isSimulation) + { + encoder.setPosition(setpoint); + } } } diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java index e20b255a..da7dbead 100644 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -29,6 +29,7 @@ public class TalonFXSwerve extends SwerveMotor * Whether the absolute encoder is integrated. */ private final boolean absoluteEncoder = false; + private double positionConversionFactor = 0; /** * Motion magic angle voltage setter. */ @@ -154,11 +155,13 @@ public void configureIntegratedEncoder(double positionConversionFactor) TalonFXConfigurator cfg = motor.getConfigurator(); cfg.refresh(configuration); + this.positionConversionFactor = positionConversionFactor; + configuration.MotionMagic = configuration.MotionMagic .withMotionMagicCruiseVelocity(100 / positionConversionFactor) .withMotionMagicAcceleration((100 / positionConversionFactor) / 0.100) .withMotionMagicExpo_kV(0.12 * positionConversionFactor) - .withMotionMagicExpo_kA(0.1); + .withMotionMagicExpo_kA(5); configuration.Feedback = configuration.Feedback .withSensorToMechanismRatio(positionConversionFactor) @@ -321,10 +324,11 @@ public void setReference(double setpoint, double feedforward, double position) if (isDriveMotor) { - motor.setControl(m_velocityVoltageSetter.withVelocity(setpoint)); + motor.setControl(m_velocityVoltageSetter.withVelocity(setpoint));//*positionConversionFactor));//setpoint*positionConversionFactor)); } else { - motor.setControl(m_angleVoltageSetter.withPosition(setpoint)); + // Motion magic takes input in rotations + motor.setControl(m_angleVoltageSetter.withPosition(setpoint/360)); } } @@ -336,7 +340,7 @@ public void setReference(double setpoint, double feedforward, double position) @Override public double getVelocity() { - return motor.getVelocity().getValue(); + return isDriveMotor ? motor.getVelocity().getValue() : motor.getVelocity().getValue() * 360; } /** @@ -347,7 +351,7 @@ public double getVelocity() @Override public double getPosition() { - return motor.getPosition().getValue(); + return isDriveMotor ? motor.getPosition().getValue() : motor.getPosition().getValue() * 360; } /** @@ -362,7 +366,7 @@ public void setPosition(double position) { position = position < 0 ? (position % 360) + 360 : position; TalonFXConfigurator cfg = motor.getConfigurator(); - cfg.setPosition(position); + cfg.setPosition(position / 360); } } diff --git a/src/main/java/swervelib/parser/json/DeviceJson.java b/src/main/java/swervelib/parser/json/DeviceJson.java index 8973478f..9b3a8aac 100644 --- a/src/main/java/swervelib/parser/json/DeviceJson.java +++ b/src/main/java/swervelib/parser/json/DeviceJson.java @@ -1,6 +1,6 @@ package swervelib.parser.json; -import com.revrobotics.SparkMaxRelativeEncoder.Type; +import com.revrobotics.SparkRelativeEncoder.Type; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.I2C; import edu.wpi.first.wpilibj.SPI; From 02c9639a845a9c9fb734c82847b5508a3f22815d Mon Sep 17 00:00:00 2001 From: DriverStation3 Date: Sat, 13 Jan 2024 11:25:26 -0800 Subject: [PATCH 08/12] tune pid --- simgui.json | 15 +++++++++++++++ .../practiceswerve/controllerproperties.json | 4 ++-- .../deploy/practiceswerve/modules/backleft.json | 3 ++- .../deploy/practiceswerve/modules/backright.json | 3 ++- .../deploy/practiceswerve/modules/frontleft.json | 3 ++- .../deploy/practiceswerve/modules/frontright.json | 3 ++- .../practiceswerve/modules/pidfproperties.json | 4 ++-- .../robot/subsystems/DrivebaseSubsystem.java | 6 +++--- src/main/java/swervelib/SwerveModule.java | 4 ++-- src/main/java/swervelib/motors/TalonFXSwerve.java | 5 +---- 10 files changed, 33 insertions(+), 17 deletions(-) diff --git a/simgui.json b/simgui.json index 35cb8934..9734fbb2 100644 --- a/simgui.json +++ b/simgui.json @@ -21,6 +21,11 @@ "/SmartDashboard/navX-Sensor[1]": "Gyro" }, "windows": { + "/SmartDashboard/Field": { + "window": { + "visible": true + } + }, "/SmartDashboard/Scheduler": { "window": { "visible": true @@ -28,6 +33,16 @@ } } }, + "NetworkTables": { + "transitory": { + "SmartDashboard": { + "Field": { + "open": true + }, + "open": true + } + } + }, "NetworkTables Info": { "visible": true } diff --git a/src/main/deploy/practiceswerve/controllerproperties.json b/src/main/deploy/practiceswerve/controllerproperties.json index c40df9f9..168c502e 100644 --- a/src/main/deploy/practiceswerve/controllerproperties.json +++ b/src/main/deploy/practiceswerve/controllerproperties.json @@ -1,7 +1,7 @@ { - "angleJoystickRadiusDeadband": 0.5, + "angleJoystickRadiusDeadband": 0.0, "heading": { - "p": 0.4, + "p": 3.0, "i": 0, "d": 0 } diff --git a/src/main/deploy/practiceswerve/modules/backleft.json b/src/main/deploy/practiceswerve/modules/backleft.json index 10e055b1..cab1ce72 100644 --- a/src/main/deploy/practiceswerve/modules/backleft.json +++ b/src/main/deploy/practiceswerve/modules/backleft.json @@ -3,7 +3,8 @@ "front": -8.5, "left": 8.5 }, - "absoluteEncoderOffset": 206.191, + "absoluteEncoderOffset": 26.191, + "absoluteEncoderInverted": false, "drive": { "type": "falcon", "id": 7, diff --git a/src/main/deploy/practiceswerve/modules/backright.json b/src/main/deploy/practiceswerve/modules/backright.json index 731ac08a..d5bb263d 100644 --- a/src/main/deploy/practiceswerve/modules/backright.json +++ b/src/main/deploy/practiceswerve/modules/backright.json @@ -3,7 +3,8 @@ "front": -8.5, "left": -8.5 }, - "absoluteEncoderOffset": 309.99, + "absoluteEncoderOffset": 129.99, + "absoluteEncoderInverted": false, "drive": { "type": "falcon", "id": 10, diff --git a/src/main/deploy/practiceswerve/modules/frontleft.json b/src/main/deploy/practiceswerve/modules/frontleft.json index 81199fee..d158bfb9 100644 --- a/src/main/deploy/practiceswerve/modules/frontleft.json +++ b/src/main/deploy/practiceswerve/modules/frontleft.json @@ -3,7 +3,8 @@ "front": 8.5, "left": 8.5 }, - "absoluteEncoderOffset": 524.971, + "absoluteEncoderOffset": 344.971, + "absoluteEncoderInverted": false, "drive": { "type": "falcon", "id": 1, diff --git a/src/main/deploy/practiceswerve/modules/frontright.json b/src/main/deploy/practiceswerve/modules/frontright.json index 64bcbfaa..3457ed44 100644 --- a/src/main/deploy/practiceswerve/modules/frontright.json +++ b/src/main/deploy/practiceswerve/modules/frontright.json @@ -3,7 +3,8 @@ "front": 8.5, "left": -8.5 }, - "absoluteEncoderOffset": 249.346, + "absoluteEncoderOffset": 69.346, + "absoluteEncoderInverted": false, "drive": { "type": "falcon", "id": 4, diff --git a/src/main/deploy/practiceswerve/modules/pidfproperties.json b/src/main/deploy/practiceswerve/modules/pidfproperties.json index 90d78d79..0474fc56 100644 --- a/src/main/deploy/practiceswerve/modules/pidfproperties.json +++ b/src/main/deploy/practiceswerve/modules/pidfproperties.json @@ -1,13 +1,13 @@ { "drive": { - "p": 2.0, + "p": 20.0, "i": 0, "d": 0, "f": 0, "iz": 0 }, "angle": { - "p": 30.0, + "p": 50.0, "i": 0, "d": 0, "f": 0, diff --git a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java index 67dc73bf..903aae17 100644 --- a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java @@ -27,7 +27,7 @@ public class DrivebaseSubsystem extends SubsystemBase { // SWERVE CONSTANTS (that aren't in deploy dir) private static final double MAX_SPEED = 0.1; - private static final double JOYSTICK_DEADBAND = 0.0; + private static final double JOYSTICK_DEADBAND = 0.05; private static final double DRIVEBASE_RADIUS = 0; // AUTO CONSTANTS @@ -64,7 +64,7 @@ public DrivebaseSubsystem() { swerveDrive.setHeadingCorrection(false); // supposed to do something? see // https://broncbotz3481.github.io/YAGSL/swervelib/SwerveDrive.html#chassisVelocityCorrection - swerveDrive.chassisVelocityCorrection = false; + swerveDrive.chassisVelocityCorrection = true; swerveDrive.synchronizeModuleEncoders(); @@ -103,7 +103,7 @@ public void drive(ChassisSpeeds speeds) { * @param fieldOriented Whether these values are field oriented */ public void drive(Translation2d translation, Rotation2d rotation, boolean fieldOriented) { - swerveDrive.drive(translation, rotation.getRadians(), fieldOriented, false); + swerveDrive.drive(translation.unaryMinus(), -rotation.getRadians(), fieldOriented, false); } /** diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 6853a2ca..c49ec41e 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -174,7 +174,7 @@ public void queueSynchronizeEncoders() */ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, boolean force) { - desiredState = SwerveModuleState.optimize(desiredState, lastState.angle); + desiredState = SwerveModuleState.optimize(desiredState, getState().angle); if (isOpenLoop) { @@ -322,7 +322,7 @@ public double getAbsolutePosition() */ public double getRelativePosition() { - return angleMotor.getPosition() * 360; + return angleMotor.getPosition(); } /** diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java index da7dbead..0d15f26e 100644 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -29,7 +29,6 @@ public class TalonFXSwerve extends SwerveMotor * Whether the absolute encoder is integrated. */ private final boolean absoluteEncoder = false; - private double positionConversionFactor = 0; /** * Motion magic angle voltage setter. */ @@ -155,8 +154,6 @@ public void configureIntegratedEncoder(double positionConversionFactor) TalonFXConfigurator cfg = motor.getConfigurator(); cfg.refresh(configuration); - this.positionConversionFactor = positionConversionFactor; - configuration.MotionMagic = configuration.MotionMagic .withMotionMagicCruiseVelocity(100 / positionConversionFactor) .withMotionMagicAcceleration((100 / positionConversionFactor) / 0.100) @@ -324,7 +321,7 @@ public void setReference(double setpoint, double feedforward, double position) if (isDriveMotor) { - motor.setControl(m_velocityVoltageSetter.withVelocity(setpoint));//*positionConversionFactor));//setpoint*positionConversionFactor)); + motor.setControl(m_velocityVoltageSetter.withVelocity(setpoint)); } else { // Motion magic takes input in rotations From 29aa22907536cce64132a10e223159d39ea1dcdf Mon Sep 17 00:00:00 2001 From: DriverStation3 Date: Tue, 16 Jan 2024 17:57:27 -0800 Subject: [PATCH 09/12] fix cosine issue in swervelib --- src/main/java/swervelib/SwerveModule.java | 4 ++-- src/main/java/swervelib/motors/TalonFXSwerve.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index c49ec41e..4b7f5a9d 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -189,14 +189,14 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, double steerMotorError = desiredState.angle.getDegrees() - getAbsolutePosition(); /* If error is close to 0 rotations, we're already there, so apply full power */ /* If the error is close to 0.25 rotations, then we're 90 degrees, so movement doesn't help us at all */ - double cosineScalar = Math.cos(Units.rotationsToRadians(steerMotorError)); + double cosineScalar = Math.cos(Units.degreesToRadians(steerMotorError)); /* Make sure we don't invert our drive, even though we shouldn't ever target over 90 degrees anyway */ if (cosineScalar < 0.0) { cosineScalar = 0.0; } - double velocity = desiredState.speedMetersPerSecond;// * (cosineScalar); + double velocity = desiredState.speedMetersPerSecond * (cosineScalar); driveMotor.setReference(velocity, 0); } diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java index 0d15f26e..70e98b23 100644 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -153,7 +153,7 @@ public void configureIntegratedEncoder(double positionConversionFactor) { TalonFXConfigurator cfg = motor.getConfigurator(); cfg.refresh(configuration); - + configuration.MotionMagic = configuration.MotionMagic .withMotionMagicCruiseVelocity(100 / positionConversionFactor) .withMotionMagicAcceleration((100 / positionConversionFactor) / 0.100) From c2e825de15863b16756c976929bf09e50f4d1fab Mon Sep 17 00:00:00 2001 From: DriverStation3 Date: Tue, 16 Jan 2024 18:02:14 -0800 Subject: [PATCH 10/12] resolve more merge conflicts --- build.gradle | 58 ---------------------------------------------------- 1 file changed, 58 deletions(-) diff --git a/build.gradle b/build.gradle index c6b9472e..01865a83 100644 --- a/build.gradle +++ b/build.gradle @@ -1,13 +1,8 @@ plugins { -<<<<<<< HEAD - id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1" -======= id "java" id "com.diffplug.spotless" version "6.15.0" id "edu.wpi.first.GradleRIO" version "2024.1.1" id "net.ltgt.errorprone" version "3.1.0" ->>>>>>> e35ae118517e251d5a2f7e3ed0ddf345befe5016 } java { @@ -15,9 +10,6 @@ java { targetCompatibility = JavaVersion.VERSION_17 } -<<<<<<< HEAD -def ROBOT_MAIN_CLASS = "frc.team2412.robot.Main" -======= java { sourceCompatibility = JavaVersion.VERSION_17 targetCompatibility = JavaVersion.VERSION_17 @@ -33,21 +25,10 @@ def getProjectBooleanProperty(String name, boolean defaultValue = false) { } def ROBOT_MAIN_CLASS = getProjectBooleanProperty('automatedTest') ? "frc.team2412.robot.test.AutomatedTestMain" : "frc.team2412.robot.Main" ->>>>>>> e35ae118517e251d5a2f7e3ed0ddf345befe5016 // Define my targets (RoboRIO) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. deploy { -<<<<<<< HEAD - targets { - roborio(getTargetTypeClass('RoboRIO')) { - // Team number is loaded either from the .wpilib/wpilib_preferences.json - // or from command line. If not found an exception will be thrown. - // You can use getTeamOrDefault(team) instead of getTeamNumber if you - // want to store a team number in this file. - team = project.frc.getTeamNumber() - debug = project.frc.getDebugOrDefault(false) -======= targets { roborio(getTargetTypeClass('RoboRIO')) { // Team number is loaded either from the .wpilib/wpilib_preferences.json @@ -56,25 +37,11 @@ deploy { // want to store a team number in this file. team = project.frc.getTeamNumber() debug = project.frc.getDebugOrDefault(false) ->>>>>>> e35ae118517e251d5a2f7e3ed0ddf345befe5016 artifacts { // First part is artifact name, 2nd is artifact type // getTargetTypeClass is a shortcut to get the class type using a string -<<<<<<< HEAD - frcJava(getArtifactTypeClass('FRCJavaArtifact')) { - } - - // Static files artifact - frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { - files = project.fileTree('src/main/deploy') - directory = '/home/lvuser/deploy' - } - } - } - } -======= frcJava(getArtifactTypeClass('FRCJavaArtifact')) { } @@ -86,7 +53,6 @@ deploy { } } } ->>>>>>> e35ae118517e251d5a2f7e3ed0ddf345befe5016 } def deployArtifact = deploy.targets.roborio.artifacts.frcJava @@ -100,12 +66,7 @@ def includeDesktopSupport = true // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 5. dependencies { -<<<<<<< HEAD - implementation wpi.java.deps.wpilib() - implementation wpi.java.vendor.java() -======= compileOnly group: "com.google.errorprone", name: "error_prone_annotations", version: "2.24.1" ->>>>>>> e35ae118517e251d5a2f7e3ed0ddf345befe5016 roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) @@ -121,10 +82,6 @@ dependencies { nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) simulationRelease wpi.sim.enableRelease() -<<<<<<< HEAD - testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' - testRuntimeOnly 'org.junit.platform:junit-platform-launcher' -======= nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop) nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) simulationRelease wpi.sim.enableRelease() @@ -133,7 +90,6 @@ dependencies { testRuntimeOnly 'org.junit.platform:junit-platform-launcher' errorprone group: "com.google.errorprone", name: "error_prone_core", version: "2.24.1" ->>>>>>> e35ae118517e251d5a2f7e3ed0ddf345befe5016 } test { @@ -150,29 +106,19 @@ test { wpi.sim.addGui().defaultEnabled = true wpi.sim.addDriverstation() -<<<<<<< HEAD -======= if (!getProjectBooleanProperty('automatedTest')) { wpi.sim.addGui().defaultEnabled = true wpi.sim.addDriverstation() } ->>>>>>> e35ae118517e251d5a2f7e3ed0ddf345befe5016 // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') // in order to make them all available at runtime. Also adding the manifest so WPILib // knows where to look for our Robot Class. jar { -<<<<<<< HEAD - from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } - from sourceSets.main.allSource - manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) - duplicatesStrategy = DuplicatesStrategy.INCLUDE -======= from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } from sourceSets.main.allSource manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) duplicatesStrategy = DuplicatesStrategy.INCLUDE ->>>>>>> e35ae118517e251d5a2f7e3ed0ddf345befe5016 } // Configure jar and deploy tasks @@ -182,9 +128,6 @@ wpi.java.configureTestTasks(test) // Configure string concat to always inline compile tasks.withType(JavaCompile) { -<<<<<<< HEAD - options.compilerArgs.add '-XDstringConcat=inline' -======= options.compilerArgs.add '-XDstringConcat=inline' } @@ -207,5 +150,4 @@ spotless { endWithNewline(); trimTrailingWhitespace(); } ->>>>>>> e35ae118517e251d5a2f7e3ed0ddf345befe5016 } From 45911c879227d58bdee6e4fbcb7aa475b641d755 Mon Sep 17 00:00:00 2001 From: DriverStation3 Date: Tue, 16 Jan 2024 18:06:00 -0800 Subject: [PATCH 11/12] resolve build.gradle file issues --- build.gradle | 41 ++++++++++++++++++++--------------------- 1 file changed, 20 insertions(+), 21 deletions(-) diff --git a/build.gradle b/build.gradle index 01865a83..9cff810e 100644 --- a/build.gradle +++ b/build.gradle @@ -5,9 +5,9 @@ plugins { id "net.ltgt.errorprone" version "3.1.0" } -java { - sourceCompatibility = JavaVersion.VERSION_17 - targetCompatibility = JavaVersion.VERSION_17 +repositories { + mavenCentral() + maven { url "https://jitpack.io" } } java { @@ -38,9 +38,9 @@ deploy { team = project.frc.getTeamNumber() debug = project.frc.getDebugOrDefault(false) - artifacts { - // First part is artifact name, 2nd is artifact type - // getTargetTypeClass is a shortcut to get the class type using a string + artifacts { + // First part is artifact name, 2nd is artifact type + // getTargetTypeClass is a shortcut to get the class type using a string frcJava(getArtifactTypeClass('FRCJavaArtifact')) { } @@ -68,19 +68,18 @@ def includeDesktopSupport = true dependencies { compileOnly group: "com.google.errorprone", name: "error_prone_annotations", version: "2.24.1" - roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) - roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) + implementation wpi.java.deps.wpilib() + implementation wpi.java.vendor.java() - roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio) - roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio) + roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) + roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) - nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) - nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) - simulationDebug wpi.sim.enableDebug() + roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio) + roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio) - nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop) - nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) - simulationRelease wpi.sim.enableRelease() + nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) + nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) + simulationDebug wpi.sim.enableDebug() nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop) nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) @@ -92,9 +91,8 @@ dependencies { errorprone group: "com.google.errorprone", name: "error_prone_core", version: "2.24.1" } -test { - useJUnitPlatform() - systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' +tasks.withType(JavaCompile).configureEach { + options.errorprone.disableWarningsInGeneratedCode = true } test { @@ -103,8 +101,6 @@ test { } // Simulation configuration (e.g. environment variables). -wpi.sim.addGui().defaultEnabled = true -wpi.sim.addDriverstation() if (!getProjectBooleanProperty('automatedTest')) { wpi.sim.addGui().defaultEnabled = true @@ -151,3 +147,6 @@ spotless { trimTrailingWhitespace(); } } +tasks.named("build") { finalizedBy("spotlessApply") } +tasks.named("deploy") { finalizedBy("spotlessApply") } +spotlessCheck.mustRunAfter(compileJava); From a3214cc1785aaf572102d1711b0e7f804dc02afe Mon Sep 17 00:00:00 2001 From: DriverStation3 Date: Tue, 16 Jan 2024 18:08:13 -0800 Subject: [PATCH 12/12] =?UTF-8?q?first=20spotless=20commit!!!!=20?= =?UTF-8?q?=F0=9F=8E=8A=E2=9C=A8=E2=9C=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- settings.gradle | 46 +- .../robot/subsystems/DrivebaseSubsystem.java | 30 +- src/main/java/swervelib/SwerveController.java | 439 ++-- src/main/java/swervelib/SwerveDrive.java | 1926 ++++++++--------- src/main/java/swervelib/SwerveModule.java | 807 ++++--- .../encoders/AnalogAbsoluteEncoderSwerve.java | 194 +- .../swervelib/encoders/CANCoderSwerve.java | 274 ++- .../swervelib/encoders/CanAndCoderSwerve.java | 165 +- .../encoders/PWMDutyCycleEncoderSwerve.java | 172 +- .../encoders/SparkMaxAnalogEncoderSwerve.java | 199 +- .../encoders/SparkMaxEncoderSwerve.java | 217 +- .../encoders/SwerveAbsoluteEncoder.java | 110 +- .../java/swervelib/encoders/package-info.java | 3 +- .../java/swervelib/imu/ADIS16448Swerve.java | 163 +- .../java/swervelib/imu/ADIS16470Swerve.java | 162 +- .../java/swervelib/imu/ADXRS450Swerve.java | 158 +- .../java/swervelib/imu/AnalogGyroSwerve.java | 170 +- src/main/java/swervelib/imu/NavXSwerve.java | 261 +-- .../java/swervelib/imu/Pigeon2Swerve.java | 210 +- src/main/java/swervelib/imu/PigeonSwerve.java | 169 +- src/main/java/swervelib/imu/SwerveIMU.java | 95 +- src/main/java/swervelib/imu/package-info.java | 4 +- src/main/java/swervelib/math/Matter.java | 57 +- src/main/java/swervelib/math/SwerveMath.java | 667 +++--- .../java/swervelib/math/package-info.java | 4 +- .../swervelib/motors/SparkFlexSwerve.java | 787 +++---- .../motors/SparkMaxBrushedMotorSwerve.java | 771 ++++--- .../java/swervelib/motors/SparkMaxSwerve.java | 785 +++---- .../java/swervelib/motors/SwerveMotor.java | 295 ++- .../java/swervelib/motors/TalonFXSwerve.java | 792 ++++--- .../java/swervelib/motors/TalonSRXSwerve.java | 756 +++---- .../java/swervelib/motors/package-info.java | 4 +- .../java/swervelib/parser/PIDFConfig.java | 164 +- .../parser/SwerveControllerConfiguration.java | 100 +- .../parser/SwerveDriveConfiguration.java | 149 +- .../parser/SwerveModuleConfiguration.java | 273 ++- .../SwerveModulePhysicalCharacteristics.java | 167 +- .../java/swervelib/parser/SwerveParser.java | 379 ++-- .../parser/deserializer/PIDFRange.java | 19 +- .../parser/deserializer/package-info.java | 4 +- .../parser/json/ControllerPropertiesJson.java | 47 +- .../swervelib/parser/json/DeviceJson.java | 297 ++- .../swervelib/parser/json/ModuleJson.java | 213 +- .../parser/json/MotorConfigDouble.java | 48 +- .../swervelib/parser/json/MotorConfigInt.java | 48 +- .../parser/json/PIDFPropertiesJson.java | 19 +- .../parser/json/PhysicalPropertiesJson.java | 79 +- .../parser/json/SwerveDriveJson.java | 24 +- .../parser/json/modules/BoolMotorJson.java | 19 +- .../parser/json/modules/LocationJson.java | 20 +- .../parser/json/modules/package-info.java | 4 +- .../swervelib/parser/json/package-info.java | 4 +- .../java/swervelib/parser/package-info.java | 4 +- .../simulation/SwerveIMUSimulation.java | 180 +- .../simulation/SwerveModuleSimulation.java | 131 +- .../swervelib/simulation/package-info.java | 4 +- .../telemetry/SwerveDriveTelemetry.java | 179 +- .../swervelib/telemetry/package-info.java | 6 +- 58 files changed, 6217 insertions(+), 7256 deletions(-) diff --git a/settings.gradle b/settings.gradle index d94f73c6..5832fbf3 100644 --- a/settings.gradle +++ b/settings.gradle @@ -1,29 +1,29 @@ import org.gradle.internal.os.OperatingSystem pluginManagement { - repositories { - mavenLocal() - gradlePluginPortal() - String frcYear = '2024' - File frcHome - if (OperatingSystem.current().isWindows()) { - String publicFolder = System.getenv('PUBLIC') - if (publicFolder == null) { - publicFolder = "C:\\Users\\Public" - } - def homeRoot = new File(publicFolder, "wpilib") - frcHome = new File(homeRoot, frcYear) - } else { - def userFolder = System.getProperty("user.home") - def homeRoot = new File(userFolder, "wpilib") - frcHome = new File(homeRoot, frcYear) - } - def frcHomeMaven = new File(frcHome, 'maven') - maven { - name 'frcHome' - url frcHomeMaven - } - } + repositories { + mavenLocal() + gradlePluginPortal() + String frcYear = '2024' + File frcHome + if (OperatingSystem.current().isWindows()) { + String publicFolder = System.getenv('PUBLIC') + if (publicFolder == null) { + publicFolder = "C:\\Users\\Public" + } + def homeRoot = new File(publicFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } else { + def userFolder = System.getProperty("user.home") + def homeRoot = new File(userFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } + def frcHomeMaven = new File(frcHome, 'maven') + maven { + name 'frcHome' + url frcHomeMaven + } + } } Properties props = System.getProperties(); diff --git a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java index 903aae17..95307102 100644 --- a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java @@ -9,8 +9,8 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.team2412.robot.Robot; @@ -80,9 +80,12 @@ public DrivebaseSubsystem() { MAX_AUTO_SPEED, DRIVEBASE_RADIUS, new ReplanningConfig()), - ()->DriverStation.getAlliance().get().equals(Alliance.Red), // flip path if on the red alliance + () -> + DriverStation.getAlliance() + .get() + .equals(Alliance.Red), // flip path if on the red alliance this); - + SwerveDriveTelemetry.verbosity = SwerveDriveTelemetry.TelemetryVerbosity.HIGH; } @@ -115,16 +118,17 @@ public void drive(Translation2d translation, Rotation2d rotation, boolean fieldO */ public Command driveJoystick( DoubleSupplier forward, DoubleSupplier strafe, Supplier rotation) { - return this.run(() -> { - Rotation2d constrainedRotation = - Rotation2d.fromRotations( - SwerveMath.applyDeadband(rotation.get().getRotations(), true, JOYSTICK_DEADBAND)); - Translation2d constrainedTranslation = - new Translation2d( - SwerveMath.applyDeadband(forward.getAsDouble(), true, JOYSTICK_DEADBAND), - SwerveMath.applyDeadband(strafe.getAsDouble(), true, JOYSTICK_DEADBAND)); - drive(constrainedTranslation, constrainedRotation, true); - }); + return this.run( + () -> { + Rotation2d constrainedRotation = + Rotation2d.fromRotations( + SwerveMath.applyDeadband(rotation.get().getRotations(), true, JOYSTICK_DEADBAND)); + Translation2d constrainedTranslation = + new Translation2d( + SwerveMath.applyDeadband(forward.getAsDouble(), true, JOYSTICK_DEADBAND), + SwerveMath.applyDeadband(strafe.getAsDouble(), true, JOYSTICK_DEADBAND)); + drive(constrainedTranslation, constrainedRotation, true); + }); } public ChassisSpeeds getRobotSpeeds() { diff --git a/src/main/java/swervelib/SwerveController.java b/src/main/java/swervelib/SwerveController.java index 0bb1fa7f..0aa487c7 100644 --- a/src/main/java/swervelib/SwerveController.java +++ b/src/main/java/swervelib/SwerveController.java @@ -6,229 +6,218 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import swervelib.parser.SwerveControllerConfiguration; -/** - * Controller class used to convert raw inputs into robot speeds. - */ -public class SwerveController -{ - - /** - * {@link SwerveControllerConfiguration} object storing data to generate the {@link PIDController} for controlling the - * robot heading, and deadband for heading joystick. - */ - public final SwerveControllerConfiguration config; - /** - * PID Controller for the robot heading. - */ - public final PIDController thetaController; // TODO: Switch to ProfilePIDController - /** - * Last angle as a scalar [-1,1] the robot was set to. - */ - public double lastAngleScalar; - /** - * {@link SlewRateLimiter} for movement in the X direction in meters/second. - */ - public SlewRateLimiter xLimiter = null; - /** - * {@link SlewRateLimiter} for movement in the Y direction in meters/second. - */ - public SlewRateLimiter yLimiter = null; - /** - * {@link SlewRateLimiter} for angular movement in radians/second. - */ - public SlewRateLimiter angleLimiter = null; - - /** - * Construct the SwerveController object which is used for determining the speeds of the robot based on controller - * input. - * - * @param cfg {@link SwerveControllerConfiguration} containing the PIDF variables for the heading PIDF. - */ - public SwerveController(SwerveControllerConfiguration cfg) - { - config = cfg; - thetaController = config.headingPIDF.createPIDController(); - thetaController.enableContinuousInput(-Math.PI, Math.PI); - lastAngleScalar = 0; - } - - /** - * Helper function to get the {@link Translation2d} of the chassis speeds given the {@link ChassisSpeeds}. - * - * @param speeds Chassis speeds. - * @return {@link Translation2d} of the speed the robot is going in. - */ - public static Translation2d getTranslation2d(ChassisSpeeds speeds) - { - return new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); - } - - /** - * Add slew rate limiters to all controls. This prevents the robot from ramping up too much. To disable a - * {@link SlewRateLimiter} set the desired one to null. - * - * @param x The {@link SlewRateLimiter} for the X velocity in meters/second. - * @param y The {@link SlewRateLimiter} for the Y velocity in meters/second. - * @param angle The {@link SlewRateLimiter} for the angular velocity in radians/second. - */ - public void addSlewRateLimiters(SlewRateLimiter x, SlewRateLimiter y, SlewRateLimiter angle) - { - xLimiter = x; - yLimiter = y; - angleLimiter = angle; - } - - /** - * Calculate the hypot deadband and check if the joystick is within it. - * - * @param x The x value for the joystick in which the deadband should be applied. - * @param y The y value for the joystick in which the deadband should be applied. - * @return Whether the values are within the deadband from - * {@link SwerveControllerConfiguration#angleJoyStickRadiusDeadband}. - */ - public boolean withinHypotDeadband(double x, double y) - { - return Math.hypot(x, y) < config.angleJoyStickRadiusDeadband; - } - - /** - * Get the chassis speeds based on controller input of 1 joystick [-1,1] and an angle. - * - * @param xInput X joystick input for the robot to move in the X direction. X = xInput * maxSpeed - * @param yInput Y joystick input for the robot to move in the Y direction. Y = yInput * - * maxSpeed; - * @param angle The desired angle of the robot in radians. - * @param currentHeadingAngleRadians The current robot heading in radians. - * @param maxSpeed Maximum speed in meters per second. - * @return {@link ChassisSpeeds} which can be sent to th Swerve Drive. - */ - public ChassisSpeeds getTargetSpeeds( - double xInput, double yInput, double angle, double currentHeadingAngleRadians, double maxSpeed) - { - // Convert joystick inputs to m/s by scaling by max linear speed. Also uses a cubic function - // to allow for precise control and fast movement. - double x = xInput * maxSpeed; - double y = yInput * maxSpeed; - - return getRawTargetSpeeds(x, y, angle, currentHeadingAngleRadians); - } - - /** - * Get the angle in radians based off of the heading joysticks. - * - * @param headingX X joystick which controls the angle of the robot. - * @param headingY Y joystick which controls the angle of the robot. - * @return angle in radians from the joystick. - */ - public double getJoystickAngle(double headingX, double headingY) - { - lastAngleScalar = - withinHypotDeadband(headingX, headingY) ? lastAngleScalar : Math.atan2(headingX, headingY); - return lastAngleScalar; - } - - /** - * Get the chassis speeds based on controller input of 2 joysticks. One for speeds in which direction. The other for - * the angle of the robot. - * - * @param xInput X joystick input for the robot to move in the X direction. - * @param yInput Y joystick input for the robot to move in the Y direction. - * @param headingX X joystick which controls the angle of the robot. - * @param headingY Y joystick which controls the angle of the robot. - * @param currentHeadingAngleRadians The current robot heading in radians. - * @param maxSpeed Maximum speed of the drive motors in meters per second, multiplier of the xInput - * and yInput. - * @return {@link ChassisSpeeds} which can be sent to th Swerve Drive. - */ - public ChassisSpeeds getTargetSpeeds( - double xInput, - double yInput, - double headingX, - double headingY, - double currentHeadingAngleRadians, - double maxSpeed) - { - // Converts the horizontal and vertical components to the commanded angle, in radians, unless - // the joystick is near - // the center (i. e. has been released), in which case the angle is held at the last valid - // joystick input (hold - // position when stick released). - double angle = - withinHypotDeadband(headingX, headingY) ? lastAngleScalar : Math.atan2(headingX, headingY); - ChassisSpeeds speeds = getTargetSpeeds(xInput, yInput, angle, currentHeadingAngleRadians, maxSpeed); - - // Used for the position hold feature - lastAngleScalar = angle; - - return speeds; - } - - /** - * Get the {@link ChassisSpeeds} based of raw speeds desired in meters/second and heading in radians. - * - * @param xSpeed X speed in meters per second. - * @param ySpeed Y speed in meters per second. - * @param omega Angular velocity in radians/second. - * @return {@link ChassisSpeeds} the robot should move to. - */ - public ChassisSpeeds getRawTargetSpeeds(double xSpeed, double ySpeed, double omega) - { - if (xLimiter != null) - { - xSpeed = xLimiter.calculate(xSpeed); - } - if (yLimiter != null) - { - ySpeed = yLimiter.calculate(ySpeed); - } - if (angleLimiter != null) - { - omega = angleLimiter.calculate(omega); - } - - return new ChassisSpeeds(xSpeed, ySpeed, omega); - } - - /** - * Get the {@link ChassisSpeeds} based of raw speeds desired in meters/second and heading in radians. - * - * @param xSpeed X speed in meters per second. - * @param ySpeed Y speed in meters per second. - * @param targetHeadingAngleRadians Target heading in radians. - * @param currentHeadingAngleRadians Current heading in radians. - * @return {@link ChassisSpeeds} the robot should move to. - */ - public ChassisSpeeds getRawTargetSpeeds(double xSpeed, double ySpeed, double targetHeadingAngleRadians, - double currentHeadingAngleRadians) - { - // Calculates an angular rate using a PIDController and the commanded angle. Returns a value between -1 and 1 - // which is then scaled to be between -maxAngularVelocity and +maxAngularVelocity. - return getRawTargetSpeeds(xSpeed, ySpeed, - thetaController.calculate(currentHeadingAngleRadians, targetHeadingAngleRadians) * - config.maxAngularVelocity); - } - - /** - * Calculate the angular velocity given the current and target heading angle in radians. - * - * @param currentHeadingAngleRadians The current heading of the robot in radians. - * @param targetHeadingAngleRadians The target heading of the robot in radians. - * @return Angular velocity in radians per second. - */ - public double headingCalculate(double currentHeadingAngleRadians, double targetHeadingAngleRadians) - { - return thetaController.calculate(currentHeadingAngleRadians, targetHeadingAngleRadians) * config.maxAngularVelocity; - } - - /** - * Set a new maximum angular velocity that is different from the auto-generated one. Modified the - * {@link SwerveControllerConfiguration#maxAngularVelocity} field which is used in the {@link SwerveController} class - * for {@link ChassisSpeeds} generation. - * - * @param angularVelocity Angular velocity in radians per second. - */ - public void setMaximumAngularVelocity(double angularVelocity) - { - config.maxAngularVelocity = angularVelocity; - } +/** Controller class used to convert raw inputs into robot speeds. */ +public class SwerveController { + + /** + * {@link SwerveControllerConfiguration} object storing data to generate the {@link PIDController} + * for controlling the robot heading, and deadband for heading joystick. + */ + public final SwerveControllerConfiguration config; + /** PID Controller for the robot heading. */ + public final PIDController thetaController; // TODO: Switch to ProfilePIDController + /** Last angle as a scalar [-1,1] the robot was set to. */ + public double lastAngleScalar; + /** {@link SlewRateLimiter} for movement in the X direction in meters/second. */ + public SlewRateLimiter xLimiter = null; + /** {@link SlewRateLimiter} for movement in the Y direction in meters/second. */ + public SlewRateLimiter yLimiter = null; + /** {@link SlewRateLimiter} for angular movement in radians/second. */ + public SlewRateLimiter angleLimiter = null; + + /** + * Construct the SwerveController object which is used for determining the speeds of the robot + * based on controller input. + * + * @param cfg {@link SwerveControllerConfiguration} containing the PIDF variables for the heading + * PIDF. + */ + public SwerveController(SwerveControllerConfiguration cfg) { + config = cfg; + thetaController = config.headingPIDF.createPIDController(); + thetaController.enableContinuousInput(-Math.PI, Math.PI); + lastAngleScalar = 0; + } + + /** + * Helper function to get the {@link Translation2d} of the chassis speeds given the {@link + * ChassisSpeeds}. + * + * @param speeds Chassis speeds. + * @return {@link Translation2d} of the speed the robot is going in. + */ + public static Translation2d getTranslation2d(ChassisSpeeds speeds) { + return new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); + } + + /** + * Add slew rate limiters to all controls. This prevents the robot from ramping up too much. To + * disable a {@link SlewRateLimiter} set the desired one to null. + * + * @param x The {@link SlewRateLimiter} for the X velocity in meters/second. + * @param y The {@link SlewRateLimiter} for the Y velocity in meters/second. + * @param angle The {@link SlewRateLimiter} for the angular velocity in radians/second. + */ + public void addSlewRateLimiters(SlewRateLimiter x, SlewRateLimiter y, SlewRateLimiter angle) { + xLimiter = x; + yLimiter = y; + angleLimiter = angle; + } + + /** + * Calculate the hypot deadband and check if the joystick is within it. + * + * @param x The x value for the joystick in which the deadband should be applied. + * @param y The y value for the joystick in which the deadband should be applied. + * @return Whether the values are within the deadband from {@link + * SwerveControllerConfiguration#angleJoyStickRadiusDeadband}. + */ + public boolean withinHypotDeadband(double x, double y) { + return Math.hypot(x, y) < config.angleJoyStickRadiusDeadband; + } + + /** + * Get the chassis speeds based on controller input of 1 joystick [-1,1] and an angle. + * + * @param xInput X joystick input for the robot to move in the X direction. X = xInput * maxSpeed + * @param yInput Y joystick input for the robot to move in the Y direction. Y = yInput * maxSpeed; + * @param angle The desired angle of the robot in radians. + * @param currentHeadingAngleRadians The current robot heading in radians. + * @param maxSpeed Maximum speed in meters per second. + * @return {@link ChassisSpeeds} which can be sent to th Swerve Drive. + */ + public ChassisSpeeds getTargetSpeeds( + double xInput, + double yInput, + double angle, + double currentHeadingAngleRadians, + double maxSpeed) { + // Convert joystick inputs to m/s by scaling by max linear speed. Also uses a cubic function + // to allow for precise control and fast movement. + double x = xInput * maxSpeed; + double y = yInput * maxSpeed; + + return getRawTargetSpeeds(x, y, angle, currentHeadingAngleRadians); + } + + /** + * Get the angle in radians based off of the heading joysticks. + * + * @param headingX X joystick which controls the angle of the robot. + * @param headingY Y joystick which controls the angle of the robot. + * @return angle in radians from the joystick. + */ + public double getJoystickAngle(double headingX, double headingY) { + lastAngleScalar = + withinHypotDeadband(headingX, headingY) ? lastAngleScalar : Math.atan2(headingX, headingY); + return lastAngleScalar; + } + + /** + * Get the chassis speeds based on controller input of 2 joysticks. One for speeds in which + * direction. The other for the angle of the robot. + * + * @param xInput X joystick input for the robot to move in the X direction. + * @param yInput Y joystick input for the robot to move in the Y direction. + * @param headingX X joystick which controls the angle of the robot. + * @param headingY Y joystick which controls the angle of the robot. + * @param currentHeadingAngleRadians The current robot heading in radians. + * @param maxSpeed Maximum speed of the drive motors in meters per second, multiplier of the + * xInput and yInput. + * @return {@link ChassisSpeeds} which can be sent to th Swerve Drive. + */ + public ChassisSpeeds getTargetSpeeds( + double xInput, + double yInput, + double headingX, + double headingY, + double currentHeadingAngleRadians, + double maxSpeed) { + // Converts the horizontal and vertical components to the commanded angle, in radians, unless + // the joystick is near + // the center (i. e. has been released), in which case the angle is held at the last valid + // joystick input (hold + // position when stick released). + double angle = + withinHypotDeadband(headingX, headingY) ? lastAngleScalar : Math.atan2(headingX, headingY); + ChassisSpeeds speeds = + getTargetSpeeds(xInput, yInput, angle, currentHeadingAngleRadians, maxSpeed); + + // Used for the position hold feature + lastAngleScalar = angle; + + return speeds; + } + + /** + * Get the {@link ChassisSpeeds} based of raw speeds desired in meters/second and heading in + * radians. + * + * @param xSpeed X speed in meters per second. + * @param ySpeed Y speed in meters per second. + * @param omega Angular velocity in radians/second. + * @return {@link ChassisSpeeds} the robot should move to. + */ + public ChassisSpeeds getRawTargetSpeeds(double xSpeed, double ySpeed, double omega) { + if (xLimiter != null) { + xSpeed = xLimiter.calculate(xSpeed); + } + if (yLimiter != null) { + ySpeed = yLimiter.calculate(ySpeed); + } + if (angleLimiter != null) { + omega = angleLimiter.calculate(omega); + } + + return new ChassisSpeeds(xSpeed, ySpeed, omega); + } + + /** + * Get the {@link ChassisSpeeds} based of raw speeds desired in meters/second and heading in + * radians. + * + * @param xSpeed X speed in meters per second. + * @param ySpeed Y speed in meters per second. + * @param targetHeadingAngleRadians Target heading in radians. + * @param currentHeadingAngleRadians Current heading in radians. + * @return {@link ChassisSpeeds} the robot should move to. + */ + public ChassisSpeeds getRawTargetSpeeds( + double xSpeed, + double ySpeed, + double targetHeadingAngleRadians, + double currentHeadingAngleRadians) { + // Calculates an angular rate using a PIDController and the commanded angle. Returns a value + // between -1 and 1 + // which is then scaled to be between -maxAngularVelocity and +maxAngularVelocity. + return getRawTargetSpeeds( + xSpeed, + ySpeed, + thetaController.calculate(currentHeadingAngleRadians, targetHeadingAngleRadians) + * config.maxAngularVelocity); + } + + /** + * Calculate the angular velocity given the current and target heading angle in radians. + * + * @param currentHeadingAngleRadians The current heading of the robot in radians. + * @param targetHeadingAngleRadians The target heading of the robot in radians. + * @return Angular velocity in radians per second. + */ + public double headingCalculate( + double currentHeadingAngleRadians, double targetHeadingAngleRadians) { + return thetaController.calculate(currentHeadingAngleRadians, targetHeadingAngleRadians) + * config.maxAngularVelocity; + } + + /** + * Set a new maximum angular velocity that is different from the auto-generated one. Modified the + * {@link SwerveControllerConfiguration#maxAngularVelocity} field which is used in the {@link + * SwerveController} class for {@link ChassisSpeeds} generation. + * + * @param angularVelocity Angular velocity in radians per second. + */ + public void setMaximumAngularVelocity(double angularVelocity) { + config.maxAngularVelocity = angularVelocity; + } } diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 0f727b0a..9141a01c 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -11,7 +11,6 @@ import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -37,1027 +36,906 @@ import swervelib.telemetry.SwerveDriveTelemetry; import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; -/** - * Swerve Drive class representing and controlling the swerve drive. - */ -public class SwerveDrive -{ - - /** - * Swerve Kinematics object. - */ - public final SwerveDriveKinematics kinematics; - /** - * Swerve drive configuration. - */ - public final SwerveDriveConfiguration swerveDriveConfiguration; - /** - * Swerve odometry. - */ - public final SwerveDrivePoseEstimator swerveDrivePoseEstimator; - /** - * Swerve modules. - */ - private final SwerveModule[] swerveModules; - /** - * WPILib {@link Notifier} to keep odometry up to date. - */ - private final Notifier odometryThread; - /** - * Odometry lock to ensure thread safety. - */ - private final Lock odometryLock = new ReentrantLock(); - /** - * Field object. - */ - public Field2d field = new Field2d(); - /** - * Swerve controller for controlling heading of the robot. - */ - public SwerveController swerveController; - /** - * Standard deviation of encoders and gyroscopes, usually should not change. (meters of position and degrees of - * rotation) - */ - public Matrix stateStdDevs = VecBuilder.fill(0.1, - 0.1, - 0.1); - /** - * The standard deviation of the vision measurement, for best accuracy calculate the standard deviation at 2 or more - * points and fit a line to it and modify this using {@link SwerveDrive#addVisionMeasurement(Pose2d, double, Matrix)} - * with the calculated optimal standard deviation. (Units should be meters per pixel). By optimizing this you can get - * vision accurate to inches instead of feet. - */ - public Matrix visionMeasurementStdDevs = VecBuilder.fill(0.9, - 0.9, - 0.9); - /** - * Invert odometry readings of drive motor positions, used as a patch for debugging currently. - */ - public boolean invertOdometry = false; - /** - * Correct chassis velocity in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)} using 254's - * correction. - */ - public boolean chassisVelocityCorrection = true; - /** - * Whether to correct heading when driving translationally. Set to true to enable. - */ - public boolean headingCorrection = false; - /** - * Swerve IMU device for sensing the heading of the robot. - */ - private SwerveIMU imu; - /** - * Simulation of the swerve drive. - */ - private SwerveIMUSimulation simIMU; - /** - * Counter to synchronize the modules relative encoder with absolute encoder when not moving. - */ - private int moduleSynchronizationCounter = 0; - /** - * The last heading set in radians. - */ - private double lastHeadingRadians = 0; - /** - * The absolute max speed that your robot can reach while translating in meters per second. - */ - private double attainableMaxTranslationalSpeedMetersPerSecond = 0; - /** - * The absolute max speed the robot can reach while rotating radians per second. - */ - private double attainableMaxRotationalVelocityRadiansPerSecond = 0; - /** - * Maximum speed of the robot in meters per second. - */ - private double maxSpeedMPS; - - /** - * Creates a new swerve drivebase subsystem. Robot is controlled via the {@link SwerveDrive#drive} method, or via the - * {@link SwerveDrive#setRawModuleStates} method. The {@link SwerveDrive#drive} method incorporates kinematics-- it - * takes a translation and rotation, as well as parameters for field-centric and closed-loop velocity control. - * {@link SwerveDrive#setRawModuleStates} takes a list of SwerveModuleStates and directly passes them to the modules. - * This subsystem also handles odometry. - * - * @param config The {@link SwerveDriveConfiguration} configuration to base the swerve drive off of. - * @param controllerConfig The {@link SwerveControllerConfiguration} to use when creating the - * {@link SwerveController}. - * @param maxSpeedMPS Maximum speed in meters per second, remember to use {@link Units#feetToMeters(double)} if - * you have feet per second! - */ - public SwerveDrive( - SwerveDriveConfiguration config, SwerveControllerConfiguration controllerConfig, double maxSpeedMPS) - { - this.maxSpeedMPS = maxSpeedMPS; - swerveDriveConfiguration = config; - swerveController = new SwerveController(controllerConfig); - // Create Kinematics from swerve module locations. - kinematics = new SwerveDriveKinematics(config.moduleLocationsMeters); - odometryThread = new Notifier(this::updateOdometry); - - // Create an integrator for angle if the robot is being simulated to emulate an IMU - // If the robot is real, instantiate the IMU instead. - if (SwerveDriveTelemetry.isSimulation) - { - simIMU = new SwerveIMUSimulation(); - } else - { - imu = config.imu; - imu.factoryDefault(); - } - - this.swerveModules = config.modules; - - // odometry = new SwerveDriveOdometry(kinematics, getYaw(), getModulePositions()); - swerveDrivePoseEstimator = - new SwerveDrivePoseEstimator( - kinematics, - getYaw(), - getModulePositions(), - new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(0)), - stateStdDevs, - visionMeasurementStdDevs); // x,y,heading in radians; Vision measurement std dev, higher=less weight - - zeroGyro(); - - // Initialize Telemetry - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal()) - { - SmartDashboard.putData("Field", field); - } - - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) - { - SwerveDriveTelemetry.maxSpeed = maxSpeedMPS; - SwerveDriveTelemetry.maxAngularVelocity = swerveController.config.maxAngularVelocity; - SwerveDriveTelemetry.moduleCount = swerveModules.length; - SwerveDriveTelemetry.sizeFrontBack = Units.metersToInches(SwerveMath.getSwerveModule(swerveModules, true, - false).moduleLocation.getX() + - SwerveMath.getSwerveModule(swerveModules, - false, - false).moduleLocation.getX()); - SwerveDriveTelemetry.sizeLeftRight = Units.metersToInches(SwerveMath.getSwerveModule(swerveModules, false, - true).moduleLocation.getY() + - SwerveMath.getSwerveModule(swerveModules, - false, - false).moduleLocation.getY()); - SwerveDriveTelemetry.wheelLocations = new double[SwerveDriveTelemetry.moduleCount * 2]; - for (SwerveModule module : swerveModules) - { - SwerveDriveTelemetry.wheelLocations[module.moduleNumber * 2] = Units.metersToInches( - module.configuration.moduleLocation.getX()); - SwerveDriveTelemetry.wheelLocations[(module.moduleNumber * 2) + 1] = Units.metersToInches( - module.configuration.moduleLocation.getY()); - } - SwerveDriveTelemetry.measuredStates = new double[SwerveDriveTelemetry.moduleCount * 2]; - SwerveDriveTelemetry.desiredStates = new double[SwerveDriveTelemetry.moduleCount * 2]; - } - - odometryThread.startPeriodic(SwerveDriveTelemetry.isSimulation ? 0.01 : 0.02); - } - - /** - * Set the odometry update period in seconds. - * - * @param period period in seconds. - */ - public void setOdometryPeriod(double period) - { - odometryThread.stop(); - odometryThread.startPeriodic(period); - } - - /** - * Stop the odometry thread in favor of manually updating odometry. - */ - public void stopOdometryThread() - { - odometryThread.stop(); - } - - /** - * Set the conversion factor for the angle/azimuth motor controller. - * - * @param conversionFactor Angle motor conversion factor for PID, should be generated from - * {@link SwerveMath#calculateDegreesPerSteeringRotation(double, double)} or calculated. - */ - public void setAngleMotorConversionFactor(double conversionFactor) - { - for (SwerveModule module : swerveModules) - { - module.setAngleMotorConversionFactor(conversionFactor); - } - } - - /** - * Set the conversion factor for the drive motor controller. - * - * @param conversionFactor Drive motor conversion factor for PID, should be generated from - * {@link SwerveMath#calculateMetersPerRotation(double, double, double)} or calculated. - */ - public void setDriveMotorConversionFactor(double conversionFactor) - { - for (SwerveModule module : swerveModules) - { - module.setDriveMotorConversionFactor(conversionFactor); - } - } - - /** - * Set the heading correction capabilities of YAGSL. - * - * @param state {@link SwerveDrive#headingCorrection} state. - */ - public void setHeadingCorrection(boolean state) - { - headingCorrection = state; - } - - /** - * Secondary method of controlling the drive base given velocity and adjusting it for field oriented use. - * - * @param velocity Velocity of the robot desired. - */ - public void driveFieldOriented(ChassisSpeeds velocity) - { - ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getYaw()); - drive(fieldOrientedVelocity); - } - - /** - * Secondary method of controlling the drive base given velocity and adjusting it for field oriented use. - * - * @param velocity Velocity of the robot desired. - * @param centerOfRotationMeters The center of rotation in meters, 0 is the center of the robot. - */ - public void driveFieldOriented(ChassisSpeeds velocity, Translation2d centerOfRotationMeters) - { - ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getYaw()); - drive(fieldOrientedVelocity, centerOfRotationMeters); - } - - /** - * Secondary method for controlling the drivebase. Given a simple {@link ChassisSpeeds} set the swerve module states, - * to achieve the goal. - * - * @param velocity The desired robot-oriented {@link ChassisSpeeds} for the robot to achieve. - */ - public void drive(ChassisSpeeds velocity) - { - drive(velocity, false, new Translation2d()); - } - - /** - * Secondary method for controlling the drivebase. Given a simple {@link ChassisSpeeds} set the swerve module states, - * to achieve the goal. - * - * @param velocity The desired robot-oriented {@link ChassisSpeeds} for the robot to achieve. - * @param centerOfRotationMeters The center of rotation in meters, 0 is the center of the robot. - */ - public void drive(ChassisSpeeds velocity, Translation2d centerOfRotationMeters) - { - drive(velocity, false, centerOfRotationMeters); - } - - /** - * The primary method for controlling the drivebase. Takes a {@link Translation2d} and a rotation rate, and calculates - * and commands module states accordingly. Can use either open-loop or closed-loop velocity control for the wheel - * velocities. Also has field- and robot-relative modes, which affect how the translation vector is used. - * - * @param translation {@link Translation2d} that is the commanded linear velocity of the robot, in meters - * per second. In robot-relative mode, positive x is torwards the bow (front) and - * positive y is torwards port (left). In field-relative mode, positive x is away from - * the alliance wall (field North) and positive y is torwards the left wall when looking - * through the driver station glass (field West). - * @param rotation Robot angular rate, in radians per second. CCW positive. Unaffected by field/robot - * relativity. - * @param fieldRelative Drive mode. True for field-relative, false for robot-relative. - * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. - * @param centerOfRotationMeters The center of rotation in meters, 0 is the center of the robot. - */ - public void drive( - Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop, - Translation2d centerOfRotationMeters) - { - // Creates a robot-relative ChassisSpeeds object, converting from field-relative speeds if - // necessary. - ChassisSpeeds velocity = - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds( - translation.getX(), translation.getY(), rotation, getYaw()) - : new ChassisSpeeds(translation.getX(), translation.getY(), rotation); - - drive(velocity, isOpenLoop, centerOfRotationMeters); - } - - /** - * The primary method for controlling the drivebase. Takes a {@link Translation2d} and a rotation rate, and calculates - * and commands module states accordingly. Can use either open-loop or closed-loop velocity control for the wheel - * velocities. Also has field- and robot-relative modes, which affect how the translation vector is used. - * - * @param translation {@link Translation2d} that is the commanded linear velocity of the robot, in meters per - * second. In robot-relative mode, positive x is torwards the bow (front) and positive y is - * torwards port (left). In field-relative mode, positive x is away from the alliance wall (field - * North) and positive y is torwards the left wall when looking through the driver station glass - * (field West). - * @param rotation Robot angular rate, in radians per second. CCW positive. Unaffected by field/robot - * relativity. - * @param fieldRelative Drive mode. True for field-relative, false for robot-relative. - * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. - */ - public void drive( - Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) - { - // Creates a robot-relative ChassisSpeeds object, converting from field-relative speeds if - // necessary. - ChassisSpeeds velocity = - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds( - translation.getX(), translation.getY(), rotation, getYaw()) - : new ChassisSpeeds(translation.getX(), translation.getY(), rotation); - - drive(velocity, isOpenLoop, new Translation2d()); - } - - /** - * The primary method for controlling the drivebase. Takes a {@link ChassisSpeeds}, and calculates and commands module - * states accordingly. Can use either open-loop or closed-loop velocity control for the wheel velocities. Also has - * field- and robot-relative modes, which affect how the translation vector is used. - * - * @param velocity The chassis speeds to set the robot to achieve. - * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. - * @param centerOfRotationMeters The center of rotation in meters, 0 is the center of the robot. - */ - public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d centerOfRotationMeters) - { - - // Thank you to Jared Russell FRC254 for Open Loop Compensation Code - // https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5 - if (chassisVelocityCorrection) - { - velocity = ChassisSpeeds.discretize(velocity, 0.02); - } - - // Heading Angular Velocity Deadband, might make a configuration option later. - // Originally made by Team 1466 Webb Robotics. - if (headingCorrection) - { - if (Math.abs(velocity.omegaRadiansPerSecond) < 0.01) - { - velocity.omegaRadiansPerSecond = - swerveController.headingCalculate(lastHeadingRadians, getYaw().getRadians()); - } else - { - lastHeadingRadians = getYaw().getRadians(); - } - } - - // Display commanded speed for testing - if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) - { - SmartDashboard.putString("RobotVelocity", velocity.toString()); - } - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) - { - SwerveDriveTelemetry.desiredChassisSpeeds[1] = velocity.vyMetersPerSecond; - SwerveDriveTelemetry.desiredChassisSpeeds[0] = velocity.vxMetersPerSecond; - SwerveDriveTelemetry.desiredChassisSpeeds[2] = Math.toDegrees(velocity.omegaRadiansPerSecond); - } - - // Calculate required module states via kinematics - SwerveModuleState[] swerveModuleStates = kinematics.toSwerveModuleStates(velocity, centerOfRotationMeters); - - setRawModuleStates(swerveModuleStates, isOpenLoop); - } - - - /** - * Set the maximum speeds for desaturation. - * - * @param attainableMaxModuleSpeedMetersPerSecond The absolute max speed that a module can reach in meters per - * second. - * @param attainableMaxTranslationalSpeedMetersPerSecond The absolute max speed that your robot can reach while - * translating in meters per second. - * @param attainableMaxRotationalVelocityRadiansPerSecond The absolute max speed the robot can reach while rotating in - * radians per second. - */ - public void setMaximumSpeeds( - double attainableMaxModuleSpeedMetersPerSecond, - double attainableMaxTranslationalSpeedMetersPerSecond, - double attainableMaxRotationalVelocityRadiansPerSecond) - { - setMaximumSpeed(attainableMaxModuleSpeedMetersPerSecond); - this.attainableMaxTranslationalSpeedMetersPerSecond = attainableMaxTranslationalSpeedMetersPerSecond; - this.attainableMaxRotationalVelocityRadiansPerSecond = attainableMaxRotationalVelocityRadiansPerSecond; - } - - /** - * Set the module states (azimuth and velocity) directly. Used primarily for auto pathing. - * - * @param desiredStates A list of SwerveModuleStates to send to the modules. - * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. - */ - private void setRawModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop) - { - // Desaturates wheel speeds - if (attainableMaxTranslationalSpeedMetersPerSecond != 0 || attainableMaxRotationalVelocityRadiansPerSecond != 0) - { - SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, getRobotVelocity(), - maxSpeedMPS, - attainableMaxTranslationalSpeedMetersPerSecond, - attainableMaxRotationalVelocityRadiansPerSecond); - } - - // Sets states - for (SwerveModule module : swerveModules) - { - module.setDesiredState(desiredStates[module.moduleNumber], isOpenLoop, false); - - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) - { - SwerveDriveTelemetry.desiredStates[module.moduleNumber * - 2] = module.lastState.angle.getDegrees(); - SwerveDriveTelemetry.desiredStates[(module.moduleNumber * 2) + - 1] = module.lastState.speedMetersPerSecond; - } - } - } - - /** - * Set the module states (azimuth and velocity) directly. Used primarily for auto paths. - * - * @param desiredStates A list of SwerveModuleStates to send to the modules. - * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. - */ - public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop) - { - setRawModuleStates(kinematics.toSwerveModuleStates(kinematics.toChassisSpeeds(desiredStates)), - isOpenLoop); - } - - /** - * Set chassis speeds with closed-loop velocity control. - * - * @param chassisSpeeds Chassis speeds to set. - */ - public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) - { - SwerveDriveTelemetry.desiredChassisSpeeds[1] = chassisSpeeds.vyMetersPerSecond; - SwerveDriveTelemetry.desiredChassisSpeeds[0] = chassisSpeeds.vxMetersPerSecond; - SwerveDriveTelemetry.desiredChassisSpeeds[2] = Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond); - - setRawModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds), false); - } - - /** - * Gets the current pose (position and rotation) of the robot, as reported by odometry. - * - * @return The robot's pose - */ - public Pose2d getPose() - { - - odometryLock.lock(); - Pose2d poseEstimation = swerveDrivePoseEstimator.getEstimatedPosition(); - odometryLock.unlock(); - return poseEstimation; - } - - /** - * Gets the current field-relative velocity (x, y and omega) of the robot - * - * @return A ChassisSpeeds object of the current field-relative velocity - */ - public ChassisSpeeds getFieldVelocity() - { - // ChassisSpeeds has a method to convert from field-relative to robot-relative speeds, - // but not the reverse. However, because this transform is a simple rotation, negating the - // angle - // given as the robot angle reverses the direction of rotation, and the conversion is reversed. - return ChassisSpeeds.fromFieldRelativeSpeeds( - kinematics.toChassisSpeeds(getStates()), getYaw().unaryMinus()); - } - - /** - * Gets the current robot-relative velocity (x, y and omega) of the robot - * - * @return A ChassisSpeeds object of the current robot-relative velocity - */ - public ChassisSpeeds getRobotVelocity() - { - return kinematics.toChassisSpeeds(getStates()); - } - - /** - * Resets odometry to the given pose. Gyro angle and module positions do not need to be reset when calling this - * method. However, if either gyro angle or module position is reset, this must be called in order for odometry to - * keep working. - * - * @param pose The pose to set the odometry to - */ - public void resetOdometry(Pose2d pose) - { - odometryLock.lock(); - swerveDrivePoseEstimator.resetPosition(pose.getRotation(), getModulePositions(), pose); - odometryLock.unlock(); - kinematics.toSwerveModuleStates(ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, 0, pose.getRotation())); - } - - /** - * Post the trajectory to the field - * - * @param trajectory the trajectory to post. - */ - public void postTrajectory(Trajectory trajectory) - { - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal()) - { - field.getObject("Trajectory").setTrajectory(trajectory); - } - } - - /** - * Gets the current module states (azimuth and velocity) - * - * @return A list of SwerveModuleStates containing the current module states - */ - public SwerveModuleState[] getStates() - { - SwerveModuleState[] states = new SwerveModuleState[swerveDriveConfiguration.moduleCount]; - for (SwerveModule module : swerveModules) - { - states[module.moduleNumber] = module.getState(); - } - return states; - } - - /** - * Gets the current module positions (azimuth and wheel position (meters)). Inverts the distance from each module if - * {@link #invertOdometry} is true. - * - * @return A list of SwerveModulePositions containg the current module positions - */ - public SwerveModulePosition[] getModulePositions() - { - SwerveModulePosition[] positions = - new SwerveModulePosition[swerveDriveConfiguration.moduleCount]; - for (SwerveModule module : swerveModules) - { - positions[module.moduleNumber] = module.getPosition(); - if (invertOdometry) - { - positions[module.moduleNumber].distanceMeters *= -1; - } - } - return positions; - } - - /** - * Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0. - */ - public void zeroGyro() - { - // Resets the real gyro or the angle accumulator, depending on whether the robot is being - // simulated - if (!SwerveDriveTelemetry.isSimulation) - { - imu.setOffset(imu.getRawRotation3d()); - } else - { - simIMU.setAngle(0); - } - swerveController.lastAngleScalar = 0; - lastHeadingRadians = 0; - resetOdometry(new Pose2d(getPose().getTranslation(), new Rotation2d())); - } - - /** - * Gets the current yaw angle of the robot, as reported by the imu. CCW positive, not wrapped. - * - * @return The yaw as a {@link Rotation2d} angle - */ - public Rotation2d getYaw() - { - // Read the imu if the robot is real or the accumulator if the robot is simulated. - if (!SwerveDriveTelemetry.isSimulation) - { - return swerveDriveConfiguration.invertedIMU - ? Rotation2d.fromRadians(imu.getRotation3d().unaryMinus().getZ()) - : Rotation2d.fromRadians(imu.getRotation3d().getZ()); - } else - { - return simIMU.getYaw(); - } - } - - /** - * Gets the current pitch angle of the robot, as reported by the imu. - * - * @return The heading as a {@link Rotation2d} angle - */ - public Rotation2d getPitch() - { - // Read the imu if the robot is real or the accumulator if the robot is simulated. - if (!SwerveDriveTelemetry.isSimulation) - { - return swerveDriveConfiguration.invertedIMU - ? Rotation2d.fromRadians(imu.getRotation3d().unaryMinus().getY()) - : Rotation2d.fromRadians(imu.getRotation3d().getY()); - } else - { - return simIMU.getPitch(); - } - } - - /** - * Gets the current roll angle of the robot, as reported by the imu. - * - * @return The heading as a {@link Rotation2d} angle - */ - public Rotation2d getRoll() - { - // Read the imu if the robot is real or the accumulator if the robot is simulated. - if (!SwerveDriveTelemetry.isSimulation) - { - return swerveDriveConfiguration.invertedIMU - ? Rotation2d.fromRadians(imu.getRotation3d().unaryMinus().getX()) - : Rotation2d.fromRadians(imu.getRotation3d().getX()); - } else - { - return simIMU.getRoll(); - } - } - - /** - * Gets the current gyro {@link Rotation3d} of the robot, as reported by the imu. - * - * @return The heading as a {@link Rotation3d} angle - */ - public Rotation3d getGyroRotation3d() - { - // Read the imu if the robot is real or the accumulator if the robot is simulated. - if (!SwerveDriveTelemetry.isSimulation) - { - return swerveDriveConfiguration.invertedIMU - ? imu.getRotation3d().unaryMinus() - : imu.getRotation3d(); - } else - { - return simIMU.getGyroRotation3d(); - } - } - - /** - * Gets current acceleration of the robot in m/s/s. If gyro unsupported returns empty. - * - * @return acceleration of the robot as a {@link Translation3d} - */ - public Optional getAccel() - { - if (!SwerveDriveTelemetry.isSimulation) - { - return imu.getAccel(); - } else - { - return simIMU.getAccel(); - } - } - - /** - * Sets the drive motors to brake/coast mode. - * - * @param brake True to set motors to brake mode, false for coast. - */ - public void setMotorIdleMode(boolean brake) - { - for (SwerveModule swerveModule : swerveModules) - { - swerveModule.setMotorBrake(brake); - } - } - - /** - * Set the maximum speed of the drive motors, modified {@link SwerveDrive#maxSpeedMPS} which is used for the - * {@link SwerveDrive#setRawModuleStates(SwerveModuleState[], boolean)} function and - * {@link SwerveController#getTargetSpeeds(double, double, double, double, double)} functions. This function overrides - * what was placed in the JSON and could damage your motor/robot if set too high or unachievable rates. - * - * @param maximumSpeed Maximum speed for the drive motors in meters / second. - * @param updateModuleFeedforward Update the swerve module feedforward to account for the new maximum speed. This - * should be true unless you have replaced the drive motor feedforward with - * {@link SwerveDrive#replaceSwerveModuleFeedforward(SimpleMotorFeedforward)} - * @param optimalVoltage Optimal voltage to use for the feedforward. - */ - public void setMaximumSpeed(double maximumSpeed, boolean updateModuleFeedforward, double optimalVoltage) - { - maxSpeedMPS = maximumSpeed; - swerveDriveConfiguration.physicalCharacteristics.optimalVoltage = optimalVoltage; - for (SwerveModule module : swerveModules) - { - if (updateModuleFeedforward) - { - module.feedforward = SwerveMath.createDriveFeedforward(optimalVoltage, - maximumSpeed, - swerveDriveConfiguration.physicalCharacteristics.wheelGripCoefficientOfFriction); - } - } - } - - /** - * Set the maximum speed of the drive motors, modified {@link SwerveDrive#maxSpeedMPS} which is used for the - * {@link SwerveDrive#setRawModuleStates(SwerveModuleState[], boolean)} function and - * {@link SwerveController#getTargetSpeeds(double, double, double, double, double)} functions. This function overrides - * what was placed in the JSON and could damage your motor/robot if set too high or unachievable rates. Overwrites the - * {@link SwerveModule#feedforward}. - * - * @param maximumSpeed Maximum speed for the drive motors in meters / second. - */ - public void setMaximumSpeed(double maximumSpeed) - { - setMaximumSpeed(maximumSpeed, true, swerveDriveConfiguration.physicalCharacteristics.optimalVoltage); - } - - /** - * Point all modules toward the robot center, thus making the robot very difficult to move. Forcing the robot to keep - * the current pose. - */ - public void lockPose() - { - // Sets states - for (SwerveModule swerveModule : swerveModules) - { - SwerveModuleState desiredState = - new SwerveModuleState(0, swerveModule.configuration.moduleLocation.getAngle()); - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) - { - SwerveDriveTelemetry.desiredStates[swerveModule.moduleNumber * 2] = - desiredState.angle.getDegrees(); - SwerveDriveTelemetry.desiredStates[(swerveModule.moduleNumber * 2) + 1] = - desiredState.speedMetersPerSecond; - } - swerveModule.setDesiredState(desiredState, false, true); - - } - - // Update kinematics because we are not using setModuleStates - kinematics.toSwerveModuleStates(new ChassisSpeeds()); - } - - /** - * Get the swerve module poses and on the field relative to the robot. - * - * @param robotPose Robot pose. - * @return Swerve module poses. - */ - public Pose2d[] getSwerveModulePoses(Pose2d robotPose) - { - Pose2d[] poseArr = new Pose2d[swerveDriveConfiguration.moduleCount]; - List poses = new ArrayList<>(); - for (SwerveModule module : swerveModules) - { - poses.add( - robotPose.plus( - new Transform2d(module.configuration.moduleLocation, module.getState().angle))); - } - return poses.toArray(poseArr); - } - - /** - * Setup the swerve module feedforward. - * - * @param feedforward Feedforward for the drive motor on swerve modules. - */ - public void replaceSwerveModuleFeedforward(SimpleMotorFeedforward feedforward) - { - for (SwerveModule swerveModule : swerveModules) - { - swerveModule.feedforward = feedforward; - } - } - - /** - * Update odometry should be run every loop. Synchronizes module absolute encoders with relative encoders - * periodically. In simulation mode will also post the pose of each module. Updates SmartDashboard with module encoder - * readings and states. - */ - public void updateOdometry() - { - odometryLock.lock(); - try - { - // Update odometry - swerveDrivePoseEstimator.update(getYaw(), getModulePositions()); - - // Update angle accumulator if the robot is simulated - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) - { - Pose2d[] modulePoses = getSwerveModulePoses(swerveDrivePoseEstimator.getEstimatedPosition()); - if (SwerveDriveTelemetry.isSimulation) - { - simIMU.updateOdometry( - kinematics, - getStates(), - modulePoses, - field); - } - - ChassisSpeeds measuredChassisSpeeds = getRobotVelocity(); - SwerveDriveTelemetry.measuredChassisSpeeds[1] = measuredChassisSpeeds.vyMetersPerSecond; - SwerveDriveTelemetry.measuredChassisSpeeds[0] = measuredChassisSpeeds.vxMetersPerSecond; - SwerveDriveTelemetry.measuredChassisSpeeds[2] = Math.toDegrees(measuredChassisSpeeds.omegaRadiansPerSecond); - SwerveDriveTelemetry.robotRotation = getYaw().getDegrees(); - } - - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal()) - { - field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition()); - } - - double sumVelocity = 0; - for (SwerveModule module : swerveModules) - { - SwerveModuleState moduleState = module.getState(); - sumVelocity += Math.abs(moduleState.speedMetersPerSecond); - if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) - { - module.updateTelemetry(); - } - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) - { - SwerveDriveTelemetry.measuredStates[module.moduleNumber * 2] = moduleState.angle.getDegrees(); - SwerveDriveTelemetry.measuredStates[(module.moduleNumber * 2) + 1] = moduleState.speedMetersPerSecond; - } - } - - // If the robot isn't moving synchronize the encoders every 100ms (Inspired by democrat's SDS - // lib) - // To ensure that everytime we initialize it works. - if (sumVelocity <= .01 && ++moduleSynchronizationCounter > 5) - { - synchronizeModuleEncoders(); - moduleSynchronizationCounter = 0; - } - - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) - { - SwerveDriveTelemetry.updateData(); - } - } catch (Exception e) - { - odometryLock.unlock(); - throw e; - } - odometryLock.unlock(); - } - - /** - * Synchronize angle motor integrated encoders with data from absolute encoders. - */ - public void synchronizeModuleEncoders() - { - for (SwerveModule module : swerveModules) - { - module.queueSynchronizeEncoders(); - } - } - - /** - * Set the gyro scope offset to a desired known rotation. Unlike {@link SwerveDrive#setGyro(Rotation3d)} it DOES NOT - * take the current rotation into account. - * - * @param offset {@link Rotation3d} known offset of the robot for gyroscope to use. - */ - public void setGyroOffset(Rotation3d offset) - { - if (SwerveDriveTelemetry.isSimulation) - { - simIMU.setAngle(offset.getZ()); - } else - { - imu.setOffset(offset); - } - } - - /** - * Add a vision measurement to the {@link SwerveDrivePoseEstimator} and update the {@link SwerveIMU} gyro reading with - * the given timestamp of the vision measurement.
IT IS HIGHLY RECOMMENDED TO UPDATE YOUR GYROSCOPE OFFSET - * AFTER USING THIS FUNCTION!
To update your gyroscope readings directly use - * {@link SwerveDrive#setGyroOffset(Rotation3d)}. - * - * @param robotPose Robot {@link Pose2d} as measured by vision. - * @param timestamp Timestamp the measurement was taken as time since startup, should be taken from - * {@link Timer#getFPGATimestamp()} or similar sources. - */ - public void addVisionMeasurement(Pose2d robotPose, double timestamp) - { - odometryLock.lock(); - swerveDrivePoseEstimator.addVisionMeasurement(robotPose, timestamp); - Pose2d newOdometry = new Pose2d(swerveDrivePoseEstimator.getEstimatedPosition().getTranslation(), - robotPose.getRotation()); - odometryLock.unlock(); - - setGyroOffset(new Rotation3d(0, 0, robotPose.getRotation().getRadians())); - resetOdometry(newOdometry); - } - - /** - * Add a vision measurement to the {@link SwerveDrivePoseEstimator} and update the {@link SwerveIMU} gyro reading with - * the given timestamp of the vision measurement. - * - * @param robotPose Robot {@link Pose2d} as measured by vision. - * @param timestamp Timestamp the measurement was taken as time since startup, should be taken from - * {@link Timer#getFPGATimestamp()} or similar sources. - * @param visionMeasurementStdDevs Vision measurement standard deviation that will be sent to the - * {@link SwerveDrivePoseEstimator}.The standard deviation of the vision measurement, - * for best accuracy calculate the standard deviation at 2 or more points and fit a - * line to it with the calculated optimal standard deviation. (Units should be meters - * per pixel). By optimizing this you can get * vision accurate to inches instead of - * feet. - */ - public void addVisionMeasurement(Pose2d robotPose, double timestamp, - Matrix visionMeasurementStdDevs) - { - this.visionMeasurementStdDevs = visionMeasurementStdDevs; - addVisionMeasurement(robotPose, timestamp); - } - - - /** - * Set the expected gyroscope angle using a {@link Rotation3d} object. To reset gyro, set to a new {@link Rotation3d} - * subtracted from the current gyroscopic readings {@link SwerveIMU#getRotation3d()}. - * - * @param gyro expected gyroscope angle as {@link Rotation3d}. - */ - public void setGyro(Rotation3d gyro) - { - if (SwerveDriveTelemetry.isSimulation) - { - setGyroOffset(simIMU.getGyroRotation3d().minus(gyro)); - } else - { - setGyroOffset(imu.getRawRotation3d().minus(gyro)); - } - } - - /** - * Helper function to get the {@link SwerveDrive#swerveController} for the {@link SwerveDrive} which can be used to - * generate {@link ChassisSpeeds} for the robot to orient it correctly given axis or angles, and apply - * {@link edu.wpi.first.math.filter.SlewRateLimiter} to given inputs. Important functions to look at are - * {@link SwerveController#getTargetSpeeds(double, double, double, double, double)}, - * {@link SwerveController#addSlewRateLimiters(SlewRateLimiter, SlewRateLimiter, SlewRateLimiter)}, - * {@link SwerveController#getRawTargetSpeeds(double, double, double)}. - * - * @return {@link SwerveController} for the {@link SwerveDrive}. - */ - public SwerveController getSwerveController() - { - return swerveController; - } - - /** - * Get the {@link SwerveModule}s associated with the {@link SwerveDrive}. - * - * @return {@link SwerveModule} array specified by configurations. - */ - public SwerveModule[] getModules() - { - return swerveDriveConfiguration.modules; - } - - /** - * Reset the drive encoders on the robot, useful when manually resetting the robot without a reboot, like in - * autonomous. - */ - public void resetDriveEncoders() - { - for (SwerveModule module : swerveModules) - { - module.configuration.driveMotor.setPosition(0); - } - } - - /** - * Pushes the Absolute Encoder offsets to the Encoder or Motor Controller, depending on type. Also removes the - * internal offsets to prevent double offsetting. - */ - public void pushOffsetsToControllers() - { - for (SwerveModule module : swerveModules) - { - module.pushOffsetsToControllers(); - } - } - - /** - * Restores Internal YAGSL Encoder offsets and sets the Encoder stored offset back to 0 - */ - public void restoreInternalOffset() - { - for (SwerveModule module : swerveModules) - { - module.restoreInternalOffset(); - } - } - +/** Swerve Drive class representing and controlling the swerve drive. */ +public class SwerveDrive { + + /** Swerve Kinematics object. */ + public final SwerveDriveKinematics kinematics; + /** Swerve drive configuration. */ + public final SwerveDriveConfiguration swerveDriveConfiguration; + /** Swerve odometry. */ + public final SwerveDrivePoseEstimator swerveDrivePoseEstimator; + /** Swerve modules. */ + private final SwerveModule[] swerveModules; + /** WPILib {@link Notifier} to keep odometry up to date. */ + private final Notifier odometryThread; + /** Odometry lock to ensure thread safety. */ + private final Lock odometryLock = new ReentrantLock(); + /** Field object. */ + public Field2d field = new Field2d(); + /** Swerve controller for controlling heading of the robot. */ + public SwerveController swerveController; + /** + * Standard deviation of encoders and gyroscopes, usually should not change. (meters of position + * and degrees of rotation) + */ + public Matrix stateStdDevs = VecBuilder.fill(0.1, 0.1, 0.1); + /** + * The standard deviation of the vision measurement, for best accuracy calculate the standard + * deviation at 2 or more points and fit a line to it and modify this using {@link + * SwerveDrive#addVisionMeasurement(Pose2d, double, Matrix)} with the calculated optimal standard + * deviation. (Units should be meters per pixel). By optimizing this you can get vision accurate + * to inches instead of feet. + */ + public Matrix visionMeasurementStdDevs = VecBuilder.fill(0.9, 0.9, 0.9); + /** Invert odometry readings of drive motor positions, used as a patch for debugging currently. */ + public boolean invertOdometry = false; + /** + * Correct chassis velocity in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)} + * using 254's correction. + */ + public boolean chassisVelocityCorrection = true; + /** Whether to correct heading when driving translationally. Set to true to enable. */ + public boolean headingCorrection = false; + /** Swerve IMU device for sensing the heading of the robot. */ + private SwerveIMU imu; + /** Simulation of the swerve drive. */ + private SwerveIMUSimulation simIMU; + /** Counter to synchronize the modules relative encoder with absolute encoder when not moving. */ + private int moduleSynchronizationCounter = 0; + /** The last heading set in radians. */ + private double lastHeadingRadians = 0; + /** The absolute max speed that your robot can reach while translating in meters per second. */ + private double attainableMaxTranslationalSpeedMetersPerSecond = 0; + /** The absolute max speed the robot can reach while rotating radians per second. */ + private double attainableMaxRotationalVelocityRadiansPerSecond = 0; + /** Maximum speed of the robot in meters per second. */ + private double maxSpeedMPS; + + /** + * Creates a new swerve drivebase subsystem. Robot is controlled via the {@link SwerveDrive#drive} + * method, or via the {@link SwerveDrive#setRawModuleStates} method. The {@link SwerveDrive#drive} + * method incorporates kinematics-- it takes a translation and rotation, as well as parameters for + * field-centric and closed-loop velocity control. {@link SwerveDrive#setRawModuleStates} takes a + * list of SwerveModuleStates and directly passes them to the modules. This subsystem also handles + * odometry. + * + * @param config The {@link SwerveDriveConfiguration} configuration to base the swerve drive off + * of. + * @param controllerConfig The {@link SwerveControllerConfiguration} to use when creating the + * {@link SwerveController}. + * @param maxSpeedMPS Maximum speed in meters per second, remember to use {@link + * Units#feetToMeters(double)} if you have feet per second! + */ + public SwerveDrive( + SwerveDriveConfiguration config, + SwerveControllerConfiguration controllerConfig, + double maxSpeedMPS) { + this.maxSpeedMPS = maxSpeedMPS; + swerveDriveConfiguration = config; + swerveController = new SwerveController(controllerConfig); + // Create Kinematics from swerve module locations. + kinematics = new SwerveDriveKinematics(config.moduleLocationsMeters); + odometryThread = new Notifier(this::updateOdometry); + + // Create an integrator for angle if the robot is being simulated to emulate an IMU + // If the robot is real, instantiate the IMU instead. + if (SwerveDriveTelemetry.isSimulation) { + simIMU = new SwerveIMUSimulation(); + } else { + imu = config.imu; + imu.factoryDefault(); + } + + this.swerveModules = config.modules; + + // odometry = new SwerveDriveOdometry(kinematics, getYaw(), getModulePositions()); + swerveDrivePoseEstimator = + new SwerveDrivePoseEstimator( + kinematics, + getYaw(), + getModulePositions(), + new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(0)), + stateStdDevs, + visionMeasurementStdDevs); // x,y,heading in radians; Vision measurement std dev, + // higher=less weight + + zeroGyro(); + + // Initialize Telemetry + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal()) { + SmartDashboard.putData("Field", field); + } + + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) { + SwerveDriveTelemetry.maxSpeed = maxSpeedMPS; + SwerveDriveTelemetry.maxAngularVelocity = swerveController.config.maxAngularVelocity; + SwerveDriveTelemetry.moduleCount = swerveModules.length; + SwerveDriveTelemetry.sizeFrontBack = + Units.metersToInches( + SwerveMath.getSwerveModule(swerveModules, true, false).moduleLocation.getX() + + SwerveMath.getSwerveModule(swerveModules, false, false).moduleLocation.getX()); + SwerveDriveTelemetry.sizeLeftRight = + Units.metersToInches( + SwerveMath.getSwerveModule(swerveModules, false, true).moduleLocation.getY() + + SwerveMath.getSwerveModule(swerveModules, false, false).moduleLocation.getY()); + SwerveDriveTelemetry.wheelLocations = new double[SwerveDriveTelemetry.moduleCount * 2]; + for (SwerveModule module : swerveModules) { + SwerveDriveTelemetry.wheelLocations[module.moduleNumber * 2] = + Units.metersToInches(module.configuration.moduleLocation.getX()); + SwerveDriveTelemetry.wheelLocations[(module.moduleNumber * 2) + 1] = + Units.metersToInches(module.configuration.moduleLocation.getY()); + } + SwerveDriveTelemetry.measuredStates = new double[SwerveDriveTelemetry.moduleCount * 2]; + SwerveDriveTelemetry.desiredStates = new double[SwerveDriveTelemetry.moduleCount * 2]; + } + + odometryThread.startPeriodic(SwerveDriveTelemetry.isSimulation ? 0.01 : 0.02); + } + + /** + * Set the odometry update period in seconds. + * + * @param period period in seconds. + */ + public void setOdometryPeriod(double period) { + odometryThread.stop(); + odometryThread.startPeriodic(period); + } + + /** Stop the odometry thread in favor of manually updating odometry. */ + public void stopOdometryThread() { + odometryThread.stop(); + } + + /** + * Set the conversion factor for the angle/azimuth motor controller. + * + * @param conversionFactor Angle motor conversion factor for PID, should be generated from {@link + * SwerveMath#calculateDegreesPerSteeringRotation(double, double)} or calculated. + */ + public void setAngleMotorConversionFactor(double conversionFactor) { + for (SwerveModule module : swerveModules) { + module.setAngleMotorConversionFactor(conversionFactor); + } + } + + /** + * Set the conversion factor for the drive motor controller. + * + * @param conversionFactor Drive motor conversion factor for PID, should be generated from {@link + * SwerveMath#calculateMetersPerRotation(double, double, double)} or calculated. + */ + public void setDriveMotorConversionFactor(double conversionFactor) { + for (SwerveModule module : swerveModules) { + module.setDriveMotorConversionFactor(conversionFactor); + } + } + + /** + * Set the heading correction capabilities of YAGSL. + * + * @param state {@link SwerveDrive#headingCorrection} state. + */ + public void setHeadingCorrection(boolean state) { + headingCorrection = state; + } + + /** + * Secondary method of controlling the drive base given velocity and adjusting it for field + * oriented use. + * + * @param velocity Velocity of the robot desired. + */ + public void driveFieldOriented(ChassisSpeeds velocity) { + ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getYaw()); + drive(fieldOrientedVelocity); + } + + /** + * Secondary method of controlling the drive base given velocity and adjusting it for field + * oriented use. + * + * @param velocity Velocity of the robot desired. + * @param centerOfRotationMeters The center of rotation in meters, 0 is the center of the robot. + */ + public void driveFieldOriented(ChassisSpeeds velocity, Translation2d centerOfRotationMeters) { + ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getYaw()); + drive(fieldOrientedVelocity, centerOfRotationMeters); + } + + /** + * Secondary method for controlling the drivebase. Given a simple {@link ChassisSpeeds} set the + * swerve module states, to achieve the goal. + * + * @param velocity The desired robot-oriented {@link ChassisSpeeds} for the robot to achieve. + */ + public void drive(ChassisSpeeds velocity) { + drive(velocity, false, new Translation2d()); + } + + /** + * Secondary method for controlling the drivebase. Given a simple {@link ChassisSpeeds} set the + * swerve module states, to achieve the goal. + * + * @param velocity The desired robot-oriented {@link ChassisSpeeds} for the robot to achieve. + * @param centerOfRotationMeters The center of rotation in meters, 0 is the center of the robot. + */ + public void drive(ChassisSpeeds velocity, Translation2d centerOfRotationMeters) { + drive(velocity, false, centerOfRotationMeters); + } + + /** + * The primary method for controlling the drivebase. Takes a {@link Translation2d} and a rotation + * rate, and calculates and commands module states accordingly. Can use either open-loop or + * closed-loop velocity control for the wheel velocities. Also has field- and robot-relative + * modes, which affect how the translation vector is used. + * + * @param translation {@link Translation2d} that is the commanded linear velocity of the robot, in + * meters per second. In robot-relative mode, positive x is torwards the bow (front) and + * positive y is torwards port (left). In field-relative mode, positive x is away from the + * alliance wall (field North) and positive y is torwards the left wall when looking through + * the driver station glass (field West). + * @param rotation Robot angular rate, in radians per second. CCW positive. Unaffected by + * field/robot relativity. + * @param fieldRelative Drive mode. True for field-relative, false for robot-relative. + * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable + * closed-loop. + * @param centerOfRotationMeters The center of rotation in meters, 0 is the center of the robot. + */ + public void drive( + Translation2d translation, + double rotation, + boolean fieldRelative, + boolean isOpenLoop, + Translation2d centerOfRotationMeters) { + // Creates a robot-relative ChassisSpeeds object, converting from field-relative speeds if + // necessary. + ChassisSpeeds velocity = + fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds( + translation.getX(), translation.getY(), rotation, getYaw()) + : new ChassisSpeeds(translation.getX(), translation.getY(), rotation); + + drive(velocity, isOpenLoop, centerOfRotationMeters); + } + + /** + * The primary method for controlling the drivebase. Takes a {@link Translation2d} and a rotation + * rate, and calculates and commands module states accordingly. Can use either open-loop or + * closed-loop velocity control for the wheel velocities. Also has field- and robot-relative + * modes, which affect how the translation vector is used. + * + * @param translation {@link Translation2d} that is the commanded linear velocity of the robot, in + * meters per second. In robot-relative mode, positive x is torwards the bow (front) and + * positive y is torwards port (left). In field-relative mode, positive x is away from the + * alliance wall (field North) and positive y is torwards the left wall when looking through + * the driver station glass (field West). + * @param rotation Robot angular rate, in radians per second. CCW positive. Unaffected by + * field/robot relativity. + * @param fieldRelative Drive mode. True for field-relative, false for robot-relative. + * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable + * closed-loop. + */ + public void drive( + Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) { + // Creates a robot-relative ChassisSpeeds object, converting from field-relative speeds if + // necessary. + ChassisSpeeds velocity = + fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds( + translation.getX(), translation.getY(), rotation, getYaw()) + : new ChassisSpeeds(translation.getX(), translation.getY(), rotation); + + drive(velocity, isOpenLoop, new Translation2d()); + } + + /** + * The primary method for controlling the drivebase. Takes a {@link ChassisSpeeds}, and calculates + * and commands module states accordingly. Can use either open-loop or closed-loop velocity + * control for the wheel velocities. Also has field- and robot-relative modes, which affect how + * the translation vector is used. + * + * @param velocity The chassis speeds to set the robot to achieve. + * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable + * closed-loop. + * @param centerOfRotationMeters The center of rotation in meters, 0 is the center of the robot. + */ + public void drive( + ChassisSpeeds velocity, boolean isOpenLoop, Translation2d centerOfRotationMeters) { + + // Thank you to Jared Russell FRC254 for Open Loop Compensation Code + // https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5 + if (chassisVelocityCorrection) { + velocity = ChassisSpeeds.discretize(velocity, 0.02); + } + + // Heading Angular Velocity Deadband, might make a configuration option later. + // Originally made by Team 1466 Webb Robotics. + if (headingCorrection) { + if (Math.abs(velocity.omegaRadiansPerSecond) < 0.01) { + velocity.omegaRadiansPerSecond = + swerveController.headingCalculate(lastHeadingRadians, getYaw().getRadians()); + } else { + lastHeadingRadians = getYaw().getRadians(); + } + } + + // Display commanded speed for testing + if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) { + SmartDashboard.putString("RobotVelocity", velocity.toString()); + } + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) { + SwerveDriveTelemetry.desiredChassisSpeeds[1] = velocity.vyMetersPerSecond; + SwerveDriveTelemetry.desiredChassisSpeeds[0] = velocity.vxMetersPerSecond; + SwerveDriveTelemetry.desiredChassisSpeeds[2] = Math.toDegrees(velocity.omegaRadiansPerSecond); + } + + // Calculate required module states via kinematics + SwerveModuleState[] swerveModuleStates = + kinematics.toSwerveModuleStates(velocity, centerOfRotationMeters); + + setRawModuleStates(swerveModuleStates, isOpenLoop); + } + + /** + * Set the maximum speeds for desaturation. + * + * @param attainableMaxModuleSpeedMetersPerSecond The absolute max speed that a module can reach + * in meters per second. + * @param attainableMaxTranslationalSpeedMetersPerSecond The absolute max speed that your robot + * can reach while translating in meters per second. + * @param attainableMaxRotationalVelocityRadiansPerSecond The absolute max speed the robot can + * reach while rotating in radians per second. + */ + public void setMaximumSpeeds( + double attainableMaxModuleSpeedMetersPerSecond, + double attainableMaxTranslationalSpeedMetersPerSecond, + double attainableMaxRotationalVelocityRadiansPerSecond) { + setMaximumSpeed(attainableMaxModuleSpeedMetersPerSecond); + this.attainableMaxTranslationalSpeedMetersPerSecond = + attainableMaxTranslationalSpeedMetersPerSecond; + this.attainableMaxRotationalVelocityRadiansPerSecond = + attainableMaxRotationalVelocityRadiansPerSecond; + } + + /** + * Set the module states (azimuth and velocity) directly. Used primarily for auto pathing. + * + * @param desiredStates A list of SwerveModuleStates to send to the modules. + * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable + * closed-loop. + */ + private void setRawModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop) { + // Desaturates wheel speeds + if (attainableMaxTranslationalSpeedMetersPerSecond != 0 + || attainableMaxRotationalVelocityRadiansPerSecond != 0) { + SwerveDriveKinematics.desaturateWheelSpeeds( + desiredStates, + getRobotVelocity(), + maxSpeedMPS, + attainableMaxTranslationalSpeedMetersPerSecond, + attainableMaxRotationalVelocityRadiansPerSecond); + } + + // Sets states + for (SwerveModule module : swerveModules) { + module.setDesiredState(desiredStates[module.moduleNumber], isOpenLoop, false); + + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) { + SwerveDriveTelemetry.desiredStates[module.moduleNumber * 2] = + module.lastState.angle.getDegrees(); + SwerveDriveTelemetry.desiredStates[(module.moduleNumber * 2) + 1] = + module.lastState.speedMetersPerSecond; + } + } + } + + /** + * Set the module states (azimuth and velocity) directly. Used primarily for auto paths. + * + * @param desiredStates A list of SwerveModuleStates to send to the modules. + * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable + * closed-loop. + */ + public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop) { + setRawModuleStates( + kinematics.toSwerveModuleStates(kinematics.toChassisSpeeds(desiredStates)), isOpenLoop); + } + + /** + * Set chassis speeds with closed-loop velocity control. + * + * @param chassisSpeeds Chassis speeds to set. + */ + public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) { + SwerveDriveTelemetry.desiredChassisSpeeds[1] = chassisSpeeds.vyMetersPerSecond; + SwerveDriveTelemetry.desiredChassisSpeeds[0] = chassisSpeeds.vxMetersPerSecond; + SwerveDriveTelemetry.desiredChassisSpeeds[2] = + Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond); + + setRawModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds), false); + } + + /** + * Gets the current pose (position and rotation) of the robot, as reported by odometry. + * + * @return The robot's pose + */ + public Pose2d getPose() { + + odometryLock.lock(); + Pose2d poseEstimation = swerveDrivePoseEstimator.getEstimatedPosition(); + odometryLock.unlock(); + return poseEstimation; + } + + /** + * Gets the current field-relative velocity (x, y and omega) of the robot + * + * @return A ChassisSpeeds object of the current field-relative velocity + */ + public ChassisSpeeds getFieldVelocity() { + // ChassisSpeeds has a method to convert from field-relative to robot-relative speeds, + // but not the reverse. However, because this transform is a simple rotation, negating the + // angle + // given as the robot angle reverses the direction of rotation, and the conversion is reversed. + return ChassisSpeeds.fromFieldRelativeSpeeds( + kinematics.toChassisSpeeds(getStates()), getYaw().unaryMinus()); + } + + /** + * Gets the current robot-relative velocity (x, y and omega) of the robot + * + * @return A ChassisSpeeds object of the current robot-relative velocity + */ + public ChassisSpeeds getRobotVelocity() { + return kinematics.toChassisSpeeds(getStates()); + } + + /** + * Resets odometry to the given pose. Gyro angle and module positions do not need to be reset when + * calling this method. However, if either gyro angle or module position is reset, this must be + * called in order for odometry to keep working. + * + * @param pose The pose to set the odometry to + */ + public void resetOdometry(Pose2d pose) { + odometryLock.lock(); + swerveDrivePoseEstimator.resetPosition(pose.getRotation(), getModulePositions(), pose); + odometryLock.unlock(); + kinematics.toSwerveModuleStates( + ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, 0, pose.getRotation())); + } + + /** + * Post the trajectory to the field + * + * @param trajectory the trajectory to post. + */ + public void postTrajectory(Trajectory trajectory) { + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal()) { + field.getObject("Trajectory").setTrajectory(trajectory); + } + } + + /** + * Gets the current module states (azimuth and velocity) + * + * @return A list of SwerveModuleStates containing the current module states + */ + public SwerveModuleState[] getStates() { + SwerveModuleState[] states = new SwerveModuleState[swerveDriveConfiguration.moduleCount]; + for (SwerveModule module : swerveModules) { + states[module.moduleNumber] = module.getState(); + } + return states; + } + + /** + * Gets the current module positions (azimuth and wheel position (meters)). Inverts the distance + * from each module if {@link #invertOdometry} is true. + * + * @return A list of SwerveModulePositions containg the current module positions + */ + public SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] positions = + new SwerveModulePosition[swerveDriveConfiguration.moduleCount]; + for (SwerveModule module : swerveModules) { + positions[module.moduleNumber] = module.getPosition(); + if (invertOdometry) { + positions[module.moduleNumber].distanceMeters *= -1; + } + } + return positions; + } + + /** + * Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0. + */ + public void zeroGyro() { + // Resets the real gyro or the angle accumulator, depending on whether the robot is being + // simulated + if (!SwerveDriveTelemetry.isSimulation) { + imu.setOffset(imu.getRawRotation3d()); + } else { + simIMU.setAngle(0); + } + swerveController.lastAngleScalar = 0; + lastHeadingRadians = 0; + resetOdometry(new Pose2d(getPose().getTranslation(), new Rotation2d())); + } + + /** + * Gets the current yaw angle of the robot, as reported by the imu. CCW positive, not wrapped. + * + * @return The yaw as a {@link Rotation2d} angle + */ + public Rotation2d getYaw() { + // Read the imu if the robot is real or the accumulator if the robot is simulated. + if (!SwerveDriveTelemetry.isSimulation) { + return swerveDriveConfiguration.invertedIMU + ? Rotation2d.fromRadians(imu.getRotation3d().unaryMinus().getZ()) + : Rotation2d.fromRadians(imu.getRotation3d().getZ()); + } else { + return simIMU.getYaw(); + } + } + + /** + * Gets the current pitch angle of the robot, as reported by the imu. + * + * @return The heading as a {@link Rotation2d} angle + */ + public Rotation2d getPitch() { + // Read the imu if the robot is real or the accumulator if the robot is simulated. + if (!SwerveDriveTelemetry.isSimulation) { + return swerveDriveConfiguration.invertedIMU + ? Rotation2d.fromRadians(imu.getRotation3d().unaryMinus().getY()) + : Rotation2d.fromRadians(imu.getRotation3d().getY()); + } else { + return simIMU.getPitch(); + } + } + + /** + * Gets the current roll angle of the robot, as reported by the imu. + * + * @return The heading as a {@link Rotation2d} angle + */ + public Rotation2d getRoll() { + // Read the imu if the robot is real or the accumulator if the robot is simulated. + if (!SwerveDriveTelemetry.isSimulation) { + return swerveDriveConfiguration.invertedIMU + ? Rotation2d.fromRadians(imu.getRotation3d().unaryMinus().getX()) + : Rotation2d.fromRadians(imu.getRotation3d().getX()); + } else { + return simIMU.getRoll(); + } + } + + /** + * Gets the current gyro {@link Rotation3d} of the robot, as reported by the imu. + * + * @return The heading as a {@link Rotation3d} angle + */ + public Rotation3d getGyroRotation3d() { + // Read the imu if the robot is real or the accumulator if the robot is simulated. + if (!SwerveDriveTelemetry.isSimulation) { + return swerveDriveConfiguration.invertedIMU + ? imu.getRotation3d().unaryMinus() + : imu.getRotation3d(); + } else { + return simIMU.getGyroRotation3d(); + } + } + + /** + * Gets current acceleration of the robot in m/s/s. If gyro unsupported returns empty. + * + * @return acceleration of the robot as a {@link Translation3d} + */ + public Optional getAccel() { + if (!SwerveDriveTelemetry.isSimulation) { + return imu.getAccel(); + } else { + return simIMU.getAccel(); + } + } + + /** + * Sets the drive motors to brake/coast mode. + * + * @param brake True to set motors to brake mode, false for coast. + */ + public void setMotorIdleMode(boolean brake) { + for (SwerveModule swerveModule : swerveModules) { + swerveModule.setMotorBrake(brake); + } + } + + /** + * Set the maximum speed of the drive motors, modified {@link SwerveDrive#maxSpeedMPS} which is + * used for the {@link SwerveDrive#setRawModuleStates(SwerveModuleState[], boolean)} function and + * {@link SwerveController#getTargetSpeeds(double, double, double, double, double)} functions. + * This function overrides what was placed in the JSON and could damage your motor/robot if set + * too high or unachievable rates. + * + * @param maximumSpeed Maximum speed for the drive motors in meters / second. + * @param updateModuleFeedforward Update the swerve module feedforward to account for the new + * maximum speed. This should be true unless you have replaced the drive motor feedforward + * with {@link SwerveDrive#replaceSwerveModuleFeedforward(SimpleMotorFeedforward)} + * @param optimalVoltage Optimal voltage to use for the feedforward. + */ + public void setMaximumSpeed( + double maximumSpeed, boolean updateModuleFeedforward, double optimalVoltage) { + maxSpeedMPS = maximumSpeed; + swerveDriveConfiguration.physicalCharacteristics.optimalVoltage = optimalVoltage; + for (SwerveModule module : swerveModules) { + if (updateModuleFeedforward) { + module.feedforward = + SwerveMath.createDriveFeedforward( + optimalVoltage, + maximumSpeed, + swerveDriveConfiguration.physicalCharacteristics.wheelGripCoefficientOfFriction); + } + } + } + + /** + * Set the maximum speed of the drive motors, modified {@link SwerveDrive#maxSpeedMPS} which is + * used for the {@link SwerveDrive#setRawModuleStates(SwerveModuleState[], boolean)} function and + * {@link SwerveController#getTargetSpeeds(double, double, double, double, double)} functions. + * This function overrides what was placed in the JSON and could damage your motor/robot if set + * too high or unachievable rates. Overwrites the {@link SwerveModule#feedforward}. + * + * @param maximumSpeed Maximum speed for the drive motors in meters / second. + */ + public void setMaximumSpeed(double maximumSpeed) { + setMaximumSpeed( + maximumSpeed, true, swerveDriveConfiguration.physicalCharacteristics.optimalVoltage); + } + + /** + * Point all modules toward the robot center, thus making the robot very difficult to move. + * Forcing the robot to keep the current pose. + */ + public void lockPose() { + // Sets states + for (SwerveModule swerveModule : swerveModules) { + SwerveModuleState desiredState = + new SwerveModuleState(0, swerveModule.configuration.moduleLocation.getAngle()); + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) { + SwerveDriveTelemetry.desiredStates[swerveModule.moduleNumber * 2] = + desiredState.angle.getDegrees(); + SwerveDriveTelemetry.desiredStates[(swerveModule.moduleNumber * 2) + 1] = + desiredState.speedMetersPerSecond; + } + swerveModule.setDesiredState(desiredState, false, true); + } + + // Update kinematics because we are not using setModuleStates + kinematics.toSwerveModuleStates(new ChassisSpeeds()); + } + + /** + * Get the swerve module poses and on the field relative to the robot. + * + * @param robotPose Robot pose. + * @return Swerve module poses. + */ + public Pose2d[] getSwerveModulePoses(Pose2d robotPose) { + Pose2d[] poseArr = new Pose2d[swerveDriveConfiguration.moduleCount]; + List poses = new ArrayList<>(); + for (SwerveModule module : swerveModules) { + poses.add( + robotPose.plus( + new Transform2d(module.configuration.moduleLocation, module.getState().angle))); + } + return poses.toArray(poseArr); + } + + /** + * Setup the swerve module feedforward. + * + * @param feedforward Feedforward for the drive motor on swerve modules. + */ + public void replaceSwerveModuleFeedforward(SimpleMotorFeedforward feedforward) { + for (SwerveModule swerveModule : swerveModules) { + swerveModule.feedforward = feedforward; + } + } + + /** + * Update odometry should be run every loop. Synchronizes module absolute encoders with relative + * encoders periodically. In simulation mode will also post the pose of each module. Updates + * SmartDashboard with module encoder readings and states. + */ + public void updateOdometry() { + odometryLock.lock(); + try { + // Update odometry + swerveDrivePoseEstimator.update(getYaw(), getModulePositions()); + + // Update angle accumulator if the robot is simulated + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) { + Pose2d[] modulePoses = + getSwerveModulePoses(swerveDrivePoseEstimator.getEstimatedPosition()); + if (SwerveDriveTelemetry.isSimulation) { + simIMU.updateOdometry(kinematics, getStates(), modulePoses, field); + } + + ChassisSpeeds measuredChassisSpeeds = getRobotVelocity(); + SwerveDriveTelemetry.measuredChassisSpeeds[1] = measuredChassisSpeeds.vyMetersPerSecond; + SwerveDriveTelemetry.measuredChassisSpeeds[0] = measuredChassisSpeeds.vxMetersPerSecond; + SwerveDriveTelemetry.measuredChassisSpeeds[2] = + Math.toDegrees(measuredChassisSpeeds.omegaRadiansPerSecond); + SwerveDriveTelemetry.robotRotation = getYaw().getDegrees(); + } + + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal()) { + field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition()); + } + + double sumVelocity = 0; + for (SwerveModule module : swerveModules) { + SwerveModuleState moduleState = module.getState(); + sumVelocity += Math.abs(moduleState.speedMetersPerSecond); + if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) { + module.updateTelemetry(); + } + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) { + SwerveDriveTelemetry.measuredStates[module.moduleNumber * 2] = + moduleState.angle.getDegrees(); + SwerveDriveTelemetry.measuredStates[(module.moduleNumber * 2) + 1] = + moduleState.speedMetersPerSecond; + } + } + + // If the robot isn't moving synchronize the encoders every 100ms (Inspired by democrat's SDS + // lib) + // To ensure that everytime we initialize it works. + if (sumVelocity <= .01 && ++moduleSynchronizationCounter > 5) { + synchronizeModuleEncoders(); + moduleSynchronizationCounter = 0; + } + + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) { + SwerveDriveTelemetry.updateData(); + } + } catch (Exception e) { + odometryLock.unlock(); + throw e; + } + odometryLock.unlock(); + } + + /** Synchronize angle motor integrated encoders with data from absolute encoders. */ + public void synchronizeModuleEncoders() { + for (SwerveModule module : swerveModules) { + module.queueSynchronizeEncoders(); + } + } + + /** + * Set the gyro scope offset to a desired known rotation. Unlike {@link + * SwerveDrive#setGyro(Rotation3d)} it DOES NOT take the current rotation into account. + * + * @param offset {@link Rotation3d} known offset of the robot for gyroscope to use. + */ + public void setGyroOffset(Rotation3d offset) { + if (SwerveDriveTelemetry.isSimulation) { + simIMU.setAngle(offset.getZ()); + } else { + imu.setOffset(offset); + } + } + + /** + * Add a vision measurement to the {@link SwerveDrivePoseEstimator} and update the {@link + * SwerveIMU} gyro reading with the given timestamp of the vision measurement.
+ * IT IS HIGHLY RECOMMENDED TO UPDATE YOUR GYROSCOPE OFFSET AFTER USING THIS FUNCTION!
+ * To update your gyroscope readings directly use {@link SwerveDrive#setGyroOffset(Rotation3d)}. + * + * @param robotPose Robot {@link Pose2d} as measured by vision. + * @param timestamp Timestamp the measurement was taken as time since startup, should be taken + * from {@link Timer#getFPGATimestamp()} or similar sources. + */ + public void addVisionMeasurement(Pose2d robotPose, double timestamp) { + odometryLock.lock(); + swerveDrivePoseEstimator.addVisionMeasurement(robotPose, timestamp); + Pose2d newOdometry = + new Pose2d( + swerveDrivePoseEstimator.getEstimatedPosition().getTranslation(), + robotPose.getRotation()); + odometryLock.unlock(); + + setGyroOffset(new Rotation3d(0, 0, robotPose.getRotation().getRadians())); + resetOdometry(newOdometry); + } + + /** + * Add a vision measurement to the {@link SwerveDrivePoseEstimator} and update the {@link + * SwerveIMU} gyro reading with the given timestamp of the vision measurement. + * + * @param robotPose Robot {@link Pose2d} as measured by vision. + * @param timestamp Timestamp the measurement was taken as time since startup, should be taken + * from {@link Timer#getFPGATimestamp()} or similar sources. + * @param visionMeasurementStdDevs Vision measurement standard deviation that will be sent to the + * {@link SwerveDrivePoseEstimator}.The standard deviation of the vision measurement, for best + * accuracy calculate the standard deviation at 2 or more points and fit a line to it with the + * calculated optimal standard deviation. (Units should be meters per pixel). By optimizing + * this you can get * vision accurate to inches instead of feet. + */ + public void addVisionMeasurement( + Pose2d robotPose, double timestamp, Matrix visionMeasurementStdDevs) { + this.visionMeasurementStdDevs = visionMeasurementStdDevs; + addVisionMeasurement(robotPose, timestamp); + } + + /** + * Set the expected gyroscope angle using a {@link Rotation3d} object. To reset gyro, set to a new + * {@link Rotation3d} subtracted from the current gyroscopic readings {@link + * SwerveIMU#getRotation3d()}. + * + * @param gyro expected gyroscope angle as {@link Rotation3d}. + */ + public void setGyro(Rotation3d gyro) { + if (SwerveDriveTelemetry.isSimulation) { + setGyroOffset(simIMU.getGyroRotation3d().minus(gyro)); + } else { + setGyroOffset(imu.getRawRotation3d().minus(gyro)); + } + } + + /** + * Helper function to get the {@link SwerveDrive#swerveController} for the {@link SwerveDrive} + * which can be used to generate {@link ChassisSpeeds} for the robot to orient it correctly given + * axis or angles, and apply {@link edu.wpi.first.math.filter.SlewRateLimiter} to given inputs. + * Important functions to look at are {@link SwerveController#getTargetSpeeds(double, double, + * double, double, double)}, {@link SwerveController#addSlewRateLimiters(SlewRateLimiter, + * SlewRateLimiter, SlewRateLimiter)}, {@link SwerveController#getRawTargetSpeeds(double, double, + * double)}. + * + * @return {@link SwerveController} for the {@link SwerveDrive}. + */ + public SwerveController getSwerveController() { + return swerveController; + } + + /** + * Get the {@link SwerveModule}s associated with the {@link SwerveDrive}. + * + * @return {@link SwerveModule} array specified by configurations. + */ + public SwerveModule[] getModules() { + return swerveDriveConfiguration.modules; + } + + /** + * Reset the drive encoders on the robot, useful when manually resetting the robot without a + * reboot, like in autonomous. + */ + public void resetDriveEncoders() { + for (SwerveModule module : swerveModules) { + module.configuration.driveMotor.setPosition(0); + } + } + + /** + * Pushes the Absolute Encoder offsets to the Encoder or Motor Controller, depending on type. Also + * removes the internal offsets to prevent double offsetting. + */ + public void pushOffsetsToControllers() { + for (SwerveModule module : swerveModules) { + module.pushOffsetsToControllers(); + } + } + + /** Restores Internal YAGSL Encoder offsets and sets the Encoder stored offset back to 0 */ + public void restoreInternalOffset() { + for (SwerveModule module : swerveModules) { + module.restoreInternalOffset(); + } + } } diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 4b7f5a9d..12d8f7d1 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -15,436 +15,379 @@ import swervelib.telemetry.SwerveDriveTelemetry; import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; -/** - * The Swerve Module class which represents and controls Swerve Modules for the swerve drive. - */ -public class SwerveModule -{ - - /** - * Swerve module configuration options. - */ - public final SwerveModuleConfiguration configuration; - /** - * Swerve Motors. - */ - private final SwerveMotor angleMotor, driveMotor; - /** - * Absolute encoder for swerve drive. - */ - private final SwerveAbsoluteEncoder absoluteEncoder; - /** - * Module number for kinematics, usually 0 to 3. front left -> front right -> back left -> back right. - */ - public int moduleNumber; - /** - * Feedforward for drive motor during closed loop control. - */ - public SimpleMotorFeedforward feedforward; - /** - * Maximum speed of the drive motors in meters per second. - */ - public double maxSpeed; - /** - * Last swerve module state applied. - */ - public SwerveModuleState lastState; - /** - * Angle offset from the absolute encoder. - */ - private double angleOffset; - /** - * Simulated swerve module. - */ - private SwerveModuleSimulation simModule; - /** - * Encoder synchronization queued. - */ - private boolean synchronizeEncoderQueued = false; - - /** - * Construct the swerve module and initialize the swerve module motors and absolute encoder. - * - * @param moduleNumber Module number for kinematics. - * @param moduleConfiguration Module constants containing CAN ID's and offsets. - * @param driveFeedforward Drive motor feedforward created by - * {@link SwerveMath#createDriveFeedforward(double, double, double)}. - */ - public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfiguration, - SimpleMotorFeedforward driveFeedforward) - { - // angle = 0; - // speed = 0; - // omega = 0; - // fakePos = 0; - this.moduleNumber = moduleNumber; - configuration = moduleConfiguration; - angleOffset = moduleConfiguration.angleOffset; - - // Initialize Feedforward for drive motor. - feedforward = driveFeedforward; - - // Create motors from configuration and reset them to defaults. - angleMotor = moduleConfiguration.angleMotor; - driveMotor = moduleConfiguration.driveMotor; - angleMotor.factoryDefaults(); - driveMotor.factoryDefaults(); - - // Configure voltage comp, current limit, and ramp rate. - angleMotor.setVoltageCompensation(configuration.physicalCharacteristics.optimalVoltage); - driveMotor.setVoltageCompensation(configuration.physicalCharacteristics.optimalVoltage); - angleMotor.setCurrentLimit(configuration.physicalCharacteristics.angleMotorCurrentLimit); - driveMotor.setCurrentLimit(configuration.physicalCharacteristics.driveMotorCurrentLimit); - angleMotor.setLoopRampRate(configuration.physicalCharacteristics.angleMotorRampRate); - driveMotor.setLoopRampRate(configuration.physicalCharacteristics.driveMotorRampRate); - - // Config angle encoders - absoluteEncoder = moduleConfiguration.absoluteEncoder; - if (absoluteEncoder != null) - { - absoluteEncoder.factoryDefault(); - absoluteEncoder.configure(moduleConfiguration.absoluteEncoderInverted); - angleMotor.setPosition(getAbsolutePosition()); - } - - // Config angle motor/controller - angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle); - angleMotor.configurePIDF(moduleConfiguration.anglePIDF); - angleMotor.configurePIDWrapping(0, 90); - angleMotor.setInverted(moduleConfiguration.angleMotorInverted); - angleMotor.setMotorBrake(false); - - // Config drive motor/controller - driveMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.drive); - driveMotor.configurePIDF(moduleConfiguration.velocityPIDF); - driveMotor.setInverted(moduleConfiguration.driveMotorInverted); - driveMotor.setMotorBrake(true); - - driveMotor.burnFlash(); - angleMotor.burnFlash(); - - if (SwerveDriveTelemetry.isSimulation) - { - simModule = new SwerveModuleSimulation(); - } - - lastState = getState(); - } - - /** - * Set the voltage compensation for the swerve module motor. - * - * @param optimalVoltage Nominal voltage for operation to output to. - */ - public void setAngleMotorVoltageCompensation(double optimalVoltage) - { - angleMotor.setVoltageCompensation(optimalVoltage); - } - - /** - * Set the voltage compensation for the swerve module motor. - * - * @param optimalVoltage Nominal voltage for operation to output to. - */ - public void setDriveMotorVoltageCompensation(double optimalVoltage) - { - driveMotor.setVoltageCompensation(optimalVoltage); - } - - - /** - * Queue synchronization of the integrated angle encoder with the absolute encoder. - */ - public void queueSynchronizeEncoders() - { - if (absoluteEncoder != null) - { - synchronizeEncoderQueued = true; - } - } - - /** - * Set the desired state of the swerve module.
WARNING: If you are not using one of the functions from - * {@link SwerveDrive} you may screw up {@link SwerveDrive#kinematics} - * - * @param desiredState Desired swerve module state. - * @param isOpenLoop Whether to use open loop (direct percent) or direct velocity control. - * @param force Disables optimizations that prevent movement in the angle motor and forces the desired state - * onto the swerve module. - */ - public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, boolean force) - { - desiredState = SwerveModuleState.optimize(desiredState, getState().angle); - - if (isOpenLoop) - { - double percentOutput = desiredState.speedMetersPerSecond / maxSpeed; - driveMotor.set(percentOutput); - } else - { - // Taken from the CTRE SwerveModule class. - // https://api.ctr-electronics.com/phoenix6/release/java/src-html/com/ctre/phoenix6/mechanisms/swerve/SwerveModule.html#line.46 - /* From FRC 900's whitepaper, we add a cosine compensator to the applied drive velocity */ - /* To reduce the "skew" that occurs when changing direction */ - double steerMotorError = desiredState.angle.getDegrees() - getAbsolutePosition(); - /* If error is close to 0 rotations, we're already there, so apply full power */ - /* If the error is close to 0.25 rotations, then we're 90 degrees, so movement doesn't help us at all */ - double cosineScalar = Math.cos(Units.degreesToRadians(steerMotorError)); - /* Make sure we don't invert our drive, even though we shouldn't ever target over 90 degrees anyway */ - if (cosineScalar < 0.0) - { - cosineScalar = 0.0; - } - - double velocity = desiredState.speedMetersPerSecond * (cosineScalar); - driveMotor.setReference(velocity, 0); - } - - /* // Not necessary anymore. - // If we are forcing the angle - if (!force) - { - // Prevents module rotation if speed is less than 1% - SwerveMath.antiJitter(desiredState, lastState, Math.min(maxSpeed, 4)); - } - */ - - // Prevent module rotation if angle is the same as the previous angle. - // Synchronize encoders if queued and send in the current position as the value from the absolute encoder. - if (absoluteEncoder != null && synchronizeEncoderQueued) - { - double absoluteEncoderPosition = getAbsolutePosition(); - angleMotor.setPosition(absoluteEncoderPosition); - angleMotor.setReference(desiredState.angle.getDegrees(), 0, absoluteEncoderPosition); - synchronizeEncoderQueued = false; - } else - { - angleMotor.setReference(desiredState.angle.getDegrees(), 0); - } - - lastState = desiredState; - - if (SwerveDriveTelemetry.isSimulation) - { - simModule.updateStateAndPosition(desiredState); - } - - if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) - { - SmartDashboard.putNumber("Module[" + configuration.name + "] Speed Setpoint", desiredState.speedMetersPerSecond); - SmartDashboard.putNumber("Module[" + configuration.name + "] Angle Setpoint", desiredState.angle.getDegrees()); - } - } - - /** - * Set the angle for the module. - * - * @param angle Angle in degrees. - */ - public void setAngle(double angle) - { - angleMotor.setReference(angle, 0); - lastState.angle = Rotation2d.fromDegrees(angle); - } - - /** - * Get the Swerve Module state. - * - * @return Current SwerveModule state. - */ - public SwerveModuleState getState() - { - double velocity; - Rotation2d azimuth; - if (!SwerveDriveTelemetry.isSimulation) - { - velocity = driveMotor.getVelocity(); - azimuth = Rotation2d.fromDegrees(getAbsolutePosition()); - } else - { - return simModule.getState(); - } - return new SwerveModuleState(velocity, azimuth); - } - - /** - * Get the position of the swerve module. - * - * @return {@link SwerveModulePosition} of the swerve module. - */ - public SwerveModulePosition getPosition() - { - double position; - Rotation2d azimuth; - if (!SwerveDriveTelemetry.isSimulation) - { - position = driveMotor.getPosition(); - azimuth = Rotation2d.fromDegrees(getAbsolutePosition()); - } else - { - return simModule.getPosition(); - } - return new SwerveModulePosition(position, azimuth); - } - - /** - * Get the absolute position. Falls back to relative position on reading failure. - * - * @return Absolute encoder angle in degrees in the range [0, 360). - */ - public double getAbsolutePosition() - { - double angle; - if (absoluteEncoder != null) - { - angle = absoluteEncoder.getAbsolutePosition() - angleOffset; - if (absoluteEncoder.readingError) - { - angle = getRelativePosition(); - } - } else - { - angle = getRelativePosition(); - } - angle %= 360; - if (angle < 0.0) - { - angle += 360; - } - - return angle; - } - - /** - * Get the relative angle in degrees. - * - * @return Angle in degrees. - */ - public double getRelativePosition() - { - return angleMotor.getPosition(); - } - - /** - * Set the brake mode. - * - * @param brake Set the brake mode. - */ - public void setMotorBrake(boolean brake) - { - driveMotor.setMotorBrake(brake); - } - - /** - * Set the conversion factor for the angle/azimuth motor controller. - * - * @param conversionFactor Angle motor conversion factor for PID, should be generated from - * {@link SwerveMath#calculateDegreesPerSteeringRotation(double, double)} or calculated. - */ - public void setAngleMotorConversionFactor(double conversionFactor) - { - angleMotor.configureIntegratedEncoder(conversionFactor); - } - - /** - * Set the conversion factor for the drive motor controller. - * - * @param conversionFactor Drive motor conversion factor for PID, should be generated from - * {@link SwerveMath#calculateMetersPerRotation(double, double, double)} or calculated. - */ - public void setDriveMotorConversionFactor(double conversionFactor) - { - driveMotor.configureIntegratedEncoder(conversionFactor); - } - - /** - * Get the angle {@link SwerveMotor} for the {@link SwerveModule}. - * - * @return {@link SwerveMotor} for the angle/steering motor of the module. - */ - public SwerveMotor getAngleMotor() - { - return angleMotor; - } - - /** - * Get the drive {@link SwerveMotor} for the {@link SwerveModule}. - * - * @return {@link SwerveMotor} for the drive motor of the module. - */ - public SwerveMotor getDriveMotor() - { - return driveMotor; - } - - /** - * Fetch the {@link SwerveModuleConfiguration} for the {@link SwerveModule} with the parsed configurations. - * - * @return {@link SwerveModuleConfiguration} for the {@link SwerveModule}. - */ - public SwerveModuleConfiguration getConfiguration() - { - return configuration; - } - - /** - * Push absolute encoder offset in the memory of the encoder or controller. Also removes the internal angle offset. - */ - public void pushOffsetsToControllers() - { - if (absoluteEncoder != null) - { - if (absoluteEncoder.setAbsoluteEncoderOffset(angleOffset)) - { - angleOffset = 0; - } else - { - DriverStation.reportWarning( - "Pushing the Absolute Encoder offset to the encoder failed on module #" + moduleNumber, false); - } - } else - { - DriverStation.reportWarning("There is no Absolute Encoder on module #" + moduleNumber, false); - } - } - - /** - * Restore internal offset in YAGSL and either sets absolute encoder offset to 0 or restores old value. - */ - public void restoreInternalOffset() - { - absoluteEncoder.setAbsoluteEncoderOffset(0); - angleOffset = configuration.angleOffset; - } - - /** - * Get if the last Absolute Encoder had a read issue, such as it does not exist. - * - * @return If the last Absolute Encoder had a read issue, or absolute encoder does not exist. - */ - public boolean getAbsoluteEncoderReadIssue() - { - if (absoluteEncoder == null) - { - return true; - } else - { - return absoluteEncoder.readingError; - } - } - - /** - * Update data sent to {@link SmartDashboard}. - */ - public void updateTelemetry() - { - if (absoluteEncoder != null) - { - SmartDashboard.putNumber("Module[" + configuration.name + "] Raw Absolute Encoder", - absoluteEncoder.getAbsolutePosition()); - } - SmartDashboard.putNumber("Module[" + configuration.name + "] Adjusted Absolute Encoder", getAbsolutePosition()); - SmartDashboard.putNumber("Module[" + configuration.name + "] Absolute Encoder Read Issue", - getAbsoluteEncoderReadIssue() ? 1 : 0); - } +/** The Swerve Module class which represents and controls Swerve Modules for the swerve drive. */ +public class SwerveModule { + + /** Swerve module configuration options. */ + public final SwerveModuleConfiguration configuration; + /** Swerve Motors. */ + private final SwerveMotor angleMotor, driveMotor; + /** Absolute encoder for swerve drive. */ + private final SwerveAbsoluteEncoder absoluteEncoder; + /** + * Module number for kinematics, usually 0 to 3. front left -> front right -> back left -> back + * right. + */ + public int moduleNumber; + /** Feedforward for drive motor during closed loop control. */ + public SimpleMotorFeedforward feedforward; + /** Maximum speed of the drive motors in meters per second. */ + public double maxSpeed; + /** Last swerve module state applied. */ + public SwerveModuleState lastState; + /** Angle offset from the absolute encoder. */ + private double angleOffset; + /** Simulated swerve module. */ + private SwerveModuleSimulation simModule; + /** Encoder synchronization queued. */ + private boolean synchronizeEncoderQueued = false; + + /** + * Construct the swerve module and initialize the swerve module motors and absolute encoder. + * + * @param moduleNumber Module number for kinematics. + * @param moduleConfiguration Module constants containing CAN ID's and offsets. + * @param driveFeedforward Drive motor feedforward created by {@link + * SwerveMath#createDriveFeedforward(double, double, double)}. + */ + public SwerveModule( + int moduleNumber, + SwerveModuleConfiguration moduleConfiguration, + SimpleMotorFeedforward driveFeedforward) { + // angle = 0; + // speed = 0; + // omega = 0; + // fakePos = 0; + this.moduleNumber = moduleNumber; + configuration = moduleConfiguration; + angleOffset = moduleConfiguration.angleOffset; + + // Initialize Feedforward for drive motor. + feedforward = driveFeedforward; + + // Create motors from configuration and reset them to defaults. + angleMotor = moduleConfiguration.angleMotor; + driveMotor = moduleConfiguration.driveMotor; + angleMotor.factoryDefaults(); + driveMotor.factoryDefaults(); + + // Configure voltage comp, current limit, and ramp rate. + angleMotor.setVoltageCompensation(configuration.physicalCharacteristics.optimalVoltage); + driveMotor.setVoltageCompensation(configuration.physicalCharacteristics.optimalVoltage); + angleMotor.setCurrentLimit(configuration.physicalCharacteristics.angleMotorCurrentLimit); + driveMotor.setCurrentLimit(configuration.physicalCharacteristics.driveMotorCurrentLimit); + angleMotor.setLoopRampRate(configuration.physicalCharacteristics.angleMotorRampRate); + driveMotor.setLoopRampRate(configuration.physicalCharacteristics.driveMotorRampRate); + + // Config angle encoders + absoluteEncoder = moduleConfiguration.absoluteEncoder; + if (absoluteEncoder != null) { + absoluteEncoder.factoryDefault(); + absoluteEncoder.configure(moduleConfiguration.absoluteEncoderInverted); + angleMotor.setPosition(getAbsolutePosition()); + } + + // Config angle motor/controller + angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle); + angleMotor.configurePIDF(moduleConfiguration.anglePIDF); + angleMotor.configurePIDWrapping(0, 90); + angleMotor.setInverted(moduleConfiguration.angleMotorInverted); + angleMotor.setMotorBrake(false); + + // Config drive motor/controller + driveMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.drive); + driveMotor.configurePIDF(moduleConfiguration.velocityPIDF); + driveMotor.setInverted(moduleConfiguration.driveMotorInverted); + driveMotor.setMotorBrake(true); + + driveMotor.burnFlash(); + angleMotor.burnFlash(); + + if (SwerveDriveTelemetry.isSimulation) { + simModule = new SwerveModuleSimulation(); + } + + lastState = getState(); + } + + /** + * Set the voltage compensation for the swerve module motor. + * + * @param optimalVoltage Nominal voltage for operation to output to. + */ + public void setAngleMotorVoltageCompensation(double optimalVoltage) { + angleMotor.setVoltageCompensation(optimalVoltage); + } + + /** + * Set the voltage compensation for the swerve module motor. + * + * @param optimalVoltage Nominal voltage for operation to output to. + */ + public void setDriveMotorVoltageCompensation(double optimalVoltage) { + driveMotor.setVoltageCompensation(optimalVoltage); + } + + /** Queue synchronization of the integrated angle encoder with the absolute encoder. */ + public void queueSynchronizeEncoders() { + if (absoluteEncoder != null) { + synchronizeEncoderQueued = true; + } + } + + /** + * Set the desired state of the swerve module.
+ * WARNING: If you are not using one of the functions from {@link SwerveDrive} you may screw up + * {@link SwerveDrive#kinematics} + * + * @param desiredState Desired swerve module state. + * @param isOpenLoop Whether to use open loop (direct percent) or direct velocity control. + * @param force Disables optimizations that prevent movement in the angle motor and forces the + * desired state onto the swerve module. + */ + public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, boolean force) { + desiredState = SwerveModuleState.optimize(desiredState, getState().angle); + + if (isOpenLoop) { + double percentOutput = desiredState.speedMetersPerSecond / maxSpeed; + driveMotor.set(percentOutput); + } else { + // Taken from the CTRE SwerveModule class. + // https://api.ctr-electronics.com/phoenix6/release/java/src-html/com/ctre/phoenix6/mechanisms/swerve/SwerveModule.html#line.46 + /* From FRC 900's whitepaper, we add a cosine compensator to the applied drive velocity */ + /* To reduce the "skew" that occurs when changing direction */ + double steerMotorError = desiredState.angle.getDegrees() - getAbsolutePosition(); + /* If error is close to 0 rotations, we're already there, so apply full power */ + /* If the error is close to 0.25 rotations, then we're 90 degrees, so movement doesn't help us at all */ + double cosineScalar = Math.cos(Units.degreesToRadians(steerMotorError)); + /* Make sure we don't invert our drive, even though we shouldn't ever target over 90 degrees anyway */ + if (cosineScalar < 0.0) { + cosineScalar = 0.0; + } + + double velocity = desiredState.speedMetersPerSecond * (cosineScalar); + driveMotor.setReference(velocity, 0); + } + + /* // Not necessary anymore. + // If we are forcing the angle + if (!force) + { + // Prevents module rotation if speed is less than 1% + SwerveMath.antiJitter(desiredState, lastState, Math.min(maxSpeed, 4)); + } + */ + + // Prevent module rotation if angle is the same as the previous angle. + // Synchronize encoders if queued and send in the current position as the value from the + // absolute encoder. + if (absoluteEncoder != null && synchronizeEncoderQueued) { + double absoluteEncoderPosition = getAbsolutePosition(); + angleMotor.setPosition(absoluteEncoderPosition); + angleMotor.setReference(desiredState.angle.getDegrees(), 0, absoluteEncoderPosition); + synchronizeEncoderQueued = false; + } else { + angleMotor.setReference(desiredState.angle.getDegrees(), 0); + } + + lastState = desiredState; + + if (SwerveDriveTelemetry.isSimulation) { + simModule.updateStateAndPosition(desiredState); + } + + if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) { + SmartDashboard.putNumber( + "Module[" + configuration.name + "] Speed Setpoint", desiredState.speedMetersPerSecond); + SmartDashboard.putNumber( + "Module[" + configuration.name + "] Angle Setpoint", desiredState.angle.getDegrees()); + } + } + + /** + * Set the angle for the module. + * + * @param angle Angle in degrees. + */ + public void setAngle(double angle) { + angleMotor.setReference(angle, 0); + lastState.angle = Rotation2d.fromDegrees(angle); + } + + /** + * Get the Swerve Module state. + * + * @return Current SwerveModule state. + */ + public SwerveModuleState getState() { + double velocity; + Rotation2d azimuth; + if (!SwerveDriveTelemetry.isSimulation) { + velocity = driveMotor.getVelocity(); + azimuth = Rotation2d.fromDegrees(getAbsolutePosition()); + } else { + return simModule.getState(); + } + return new SwerveModuleState(velocity, azimuth); + } + + /** + * Get the position of the swerve module. + * + * @return {@link SwerveModulePosition} of the swerve module. + */ + public SwerveModulePosition getPosition() { + double position; + Rotation2d azimuth; + if (!SwerveDriveTelemetry.isSimulation) { + position = driveMotor.getPosition(); + azimuth = Rotation2d.fromDegrees(getAbsolutePosition()); + } else { + return simModule.getPosition(); + } + return new SwerveModulePosition(position, azimuth); + } + + /** + * Get the absolute position. Falls back to relative position on reading failure. + * + * @return Absolute encoder angle in degrees in the range [0, 360). + */ + public double getAbsolutePosition() { + double angle; + if (absoluteEncoder != null) { + angle = absoluteEncoder.getAbsolutePosition() - angleOffset; + if (absoluteEncoder.readingError) { + angle = getRelativePosition(); + } + } else { + angle = getRelativePosition(); + } + angle %= 360; + if (angle < 0.0) { + angle += 360; + } + + return angle; + } + + /** + * Get the relative angle in degrees. + * + * @return Angle in degrees. + */ + public double getRelativePosition() { + return angleMotor.getPosition(); + } + + /** + * Set the brake mode. + * + * @param brake Set the brake mode. + */ + public void setMotorBrake(boolean brake) { + driveMotor.setMotorBrake(brake); + } + + /** + * Set the conversion factor for the angle/azimuth motor controller. + * + * @param conversionFactor Angle motor conversion factor for PID, should be generated from {@link + * SwerveMath#calculateDegreesPerSteeringRotation(double, double)} or calculated. + */ + public void setAngleMotorConversionFactor(double conversionFactor) { + angleMotor.configureIntegratedEncoder(conversionFactor); + } + + /** + * Set the conversion factor for the drive motor controller. + * + * @param conversionFactor Drive motor conversion factor for PID, should be generated from {@link + * SwerveMath#calculateMetersPerRotation(double, double, double)} or calculated. + */ + public void setDriveMotorConversionFactor(double conversionFactor) { + driveMotor.configureIntegratedEncoder(conversionFactor); + } + + /** + * Get the angle {@link SwerveMotor} for the {@link SwerveModule}. + * + * @return {@link SwerveMotor} for the angle/steering motor of the module. + */ + public SwerveMotor getAngleMotor() { + return angleMotor; + } + + /** + * Get the drive {@link SwerveMotor} for the {@link SwerveModule}. + * + * @return {@link SwerveMotor} for the drive motor of the module. + */ + public SwerveMotor getDriveMotor() { + return driveMotor; + } + + /** + * Fetch the {@link SwerveModuleConfiguration} for the {@link SwerveModule} with the parsed + * configurations. + * + * @return {@link SwerveModuleConfiguration} for the {@link SwerveModule}. + */ + public SwerveModuleConfiguration getConfiguration() { + return configuration; + } + + /** + * Push absolute encoder offset in the memory of the encoder or controller. Also removes the + * internal angle offset. + */ + public void pushOffsetsToControllers() { + if (absoluteEncoder != null) { + if (absoluteEncoder.setAbsoluteEncoderOffset(angleOffset)) { + angleOffset = 0; + } else { + DriverStation.reportWarning( + "Pushing the Absolute Encoder offset to the encoder failed on module #" + moduleNumber, + false); + } + } else { + DriverStation.reportWarning("There is no Absolute Encoder on module #" + moduleNumber, false); + } + } + + /** + * Restore internal offset in YAGSL and either sets absolute encoder offset to 0 or restores old + * value. + */ + public void restoreInternalOffset() { + absoluteEncoder.setAbsoluteEncoderOffset(0); + angleOffset = configuration.angleOffset; + } + + /** + * Get if the last Absolute Encoder had a read issue, such as it does not exist. + * + * @return If the last Absolute Encoder had a read issue, or absolute encoder does not exist. + */ + public boolean getAbsoluteEncoderReadIssue() { + if (absoluteEncoder == null) { + return true; + } else { + return absoluteEncoder.readingError; + } + } + + /** Update data sent to {@link SmartDashboard}. */ + public void updateTelemetry() { + if (absoluteEncoder != null) { + SmartDashboard.putNumber( + "Module[" + configuration.name + "] Raw Absolute Encoder", + absoluteEncoder.getAbsolutePosition()); + } + SmartDashboard.putNumber( + "Module[" + configuration.name + "] Adjusted Absolute Encoder", getAbsolutePosition()); + SmartDashboard.putNumber( + "Module[" + configuration.name + "] Absolute Encoder Read Issue", + getAbsoluteEncoderReadIssue() ? 1 : 0); + } } diff --git a/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java b/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java index 1b830b1b..bb5deeb5 100644 --- a/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java @@ -4,117 +4,103 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; -/** - * Swerve Absolute Encoder for Thrifty Encoders and other analog encoders. - */ -public class AnalogAbsoluteEncoderSwerve extends SwerveAbsoluteEncoder -{ - // Entire class inspired by 5010 - // Source: https://github.com/FRC5010/FRCLibrary/blob/main/FRC5010Example2023/src/main/java/frc/robot/FRC5010/sensors/AnalogInput5010.java - /** - * Encoder as Analog Input. - */ - public AnalogInput encoder; - /** - * Inversion state of the encoder. - */ - private boolean inverted = false; +/** Swerve Absolute Encoder for Thrifty Encoders and other analog encoders. */ +public class AnalogAbsoluteEncoderSwerve extends SwerveAbsoluteEncoder { + // Entire class inspired by 5010 + // Source: + // https://github.com/FRC5010/FRCLibrary/blob/main/FRC5010Example2023/src/main/java/frc/robot/FRC5010/sensors/AnalogInput5010.java + /** Encoder as Analog Input. */ + public AnalogInput encoder; + /** Inversion state of the encoder. */ + private boolean inverted = false; - /** - * Construct the Thrifty Encoder as a Swerve Absolute Encoder. - * - * @param encoder Encoder to construct. - */ - public AnalogAbsoluteEncoderSwerve(AnalogInput encoder) - { - this.encoder = encoder; - } + /** + * Construct the Thrifty Encoder as a Swerve Absolute Encoder. + * + * @param encoder Encoder to construct. + */ + public AnalogAbsoluteEncoderSwerve(AnalogInput encoder) { + this.encoder = encoder; + } - /** - * Construct the Encoder given the analog input channel. - * - * @param channel Analog Input channel of which the encoder resides. - */ - public AnalogAbsoluteEncoderSwerve(int channel) - { - this(new AnalogInput(channel)); - } + /** + * Construct the Encoder given the analog input channel. + * + * @param channel Analog Input channel of which the encoder resides. + */ + public AnalogAbsoluteEncoderSwerve(int channel) { + this(new AnalogInput(channel)); + } - /** - * Reset the encoder to factory defaults. - */ - @Override - public void factoryDefault() - { - // Do nothing - } + /** Reset the encoder to factory defaults. */ + @Override + public void factoryDefault() { + // Do nothing + } - /** - * Clear sticky faults on the encoder. - */ - @Override - public void clearStickyFaults() - { - // Do nothing - } + /** Clear sticky faults on the encoder. */ + @Override + public void clearStickyFaults() { + // Do nothing + } - /** - * Configure the absolute encoder to read from [0, 360) per second. - * - * @param inverted Whether the encoder is inverted. - */ - @Override - public void configure(boolean inverted) - { - this.inverted = inverted; - } + /** + * Configure the absolute encoder to read from [0, 360) per second. + * + * @param inverted Whether the encoder is inverted. + */ + @Override + public void configure(boolean inverted) { + this.inverted = inverted; + } - /** - * Get the absolute position of the encoder. - * - * @return Absolute position in degrees from [0, 360). - */ - @Override - public double getAbsolutePosition() - { - return (inverted ? -1.0 : 1.0) * (encoder.getAverageVoltage() / RobotController.getVoltage5V()) * 360; - } + /** + * Get the absolute position of the encoder. + * + * @return Absolute position in degrees from [0, 360). + */ + @Override + public double getAbsolutePosition() { + return (inverted ? -1.0 : 1.0) + * (encoder.getAverageVoltage() / RobotController.getVoltage5V()) + * 360; + } - /** - * Get the instantiated absolute encoder Object. - * - * @return Absolute encoder object. - */ - @Override - public Object getAbsoluteEncoder() - { - return encoder; - } + /** + * Get the instantiated absolute encoder Object. + * + * @return Absolute encoder object. + */ + @Override + public Object getAbsoluteEncoder() { + return encoder; + } - /** - * Cannot Set the offset of an Analog Absolute Encoder. - * - * @param offset the offset the Absolute Encoder uses as the zero point. - * @return Will always be false as setting the offset is unsupported of an Analog absolute encoder. - */ - @Override - public boolean setAbsoluteEncoderOffset(double offset) - { - //Do Nothing - DriverStation.reportWarning( - "Cannot Set Absolute Encoder Offset of Analog Encoders Channel #" + encoder.getChannel(), false); - return false; - } + /** + * Cannot Set the offset of an Analog Absolute Encoder. + * + * @param offset the offset the Absolute Encoder uses as the zero point. + * @return Will always be false as setting the offset is unsupported of an Analog absolute + * encoder. + */ + @Override + public boolean setAbsoluteEncoderOffset(double offset) { + // Do Nothing + DriverStation.reportWarning( + "Cannot Set Absolute Encoder Offset of Analog Encoders Channel #" + encoder.getChannel(), + false); + return false; + } - /** - * Get the velocity in degrees/sec. - * - * @return velocity in degrees/sec. - */ - @Override - public double getVelocity() - { - DriverStation.reportWarning("The Analog Absolute encoder may not report accurate velocities!", true); - return encoder.getValue(); - } + /** + * Get the velocity in degrees/sec. + * + * @return velocity in degrees/sec. + */ + @Override + public double getVelocity() { + DriverStation.reportWarning( + "The Analog Absolute encoder may not report accurate velocities!", true); + return encoder.getValue(); + } } diff --git a/src/main/java/swervelib/encoders/CANCoderSwerve.java b/src/main/java/swervelib/encoders/CANCoderSwerve.java index 5d8c3283..2970658f 100644 --- a/src/main/java/swervelib/encoders/CANCoderSwerve.java +++ b/src/main/java/swervelib/encoders/CANCoderSwerve.java @@ -11,161 +11,147 @@ import com.ctre.phoenix6.signals.SensorDirectionValue; import edu.wpi.first.wpilibj.DriverStation; -/** - * Swerve Absolute Encoder for CTRE CANCoders. - */ -public class CANCoderSwerve extends SwerveAbsoluteEncoder -{ +/** Swerve Absolute Encoder for CTRE CANCoders. */ +public class CANCoderSwerve extends SwerveAbsoluteEncoder { - /** - * CANCoder with WPILib sendable and support. - */ - public CANcoder encoder; + /** CANCoder with WPILib sendable and support. */ + public CANcoder encoder; - /** - * Initialize the CANCoder on the standard CANBus. - * - * @param id CAN ID. - */ - public CANCoderSwerve(int id) - { - encoder = new CANcoder(id); - } + /** + * Initialize the CANCoder on the standard CANBus. + * + * @param id CAN ID. + */ + public CANCoderSwerve(int id) { + encoder = new CANcoder(id); + } - /** - * Initialize the CANCoder on the CANivore. - * - * @param id CAN ID. - * @param canbus CAN bus to initialize it on. - */ - public CANCoderSwerve(int id, String canbus) - { - encoder = new CANcoder(id, canbus); - } + /** + * Initialize the CANCoder on the CANivore. + * + * @param id CAN ID. + * @param canbus CAN bus to initialize it on. + */ + public CANCoderSwerve(int id, String canbus) { + encoder = new CANcoder(id, canbus); + } - /** - * Reset the encoder to factory defaults. - */ - @Override - public void factoryDefault() - { - encoder.getConfigurator().apply(new CANcoderConfiguration()); - } + /** Reset the encoder to factory defaults. */ + @Override + public void factoryDefault() { + encoder.getConfigurator().apply(new CANcoderConfiguration()); + } - /** - * Clear sticky faults on the encoder. - */ - @Override - public void clearStickyFaults() - { - encoder.clearStickyFaults(); - } + /** Clear sticky faults on the encoder. */ + @Override + public void clearStickyFaults() { + encoder.clearStickyFaults(); + } - /** - * Configure the absolute encoder to read from [0, 360) per second. - * - * @param inverted Whether the encoder is inverted. - */ - @Override - public void configure(boolean inverted) - { - CANcoderConfigurator cfg = encoder.getConfigurator(); - MagnetSensorConfigs magnetSensorConfiguration = new MagnetSensorConfigs(); - cfg.refresh(magnetSensorConfiguration); - cfg.apply(magnetSensorConfiguration - .withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1) - .withSensorDirection(inverted ? SensorDirectionValue.Clockwise_Positive - : SensorDirectionValue.CounterClockwise_Positive)); - } + /** + * Configure the absolute encoder to read from [0, 360) per second. + * + * @param inverted Whether the encoder is inverted. + */ + @Override + public void configure(boolean inverted) { + CANcoderConfigurator cfg = encoder.getConfigurator(); + MagnetSensorConfigs magnetSensorConfiguration = new MagnetSensorConfigs(); + cfg.refresh(magnetSensorConfiguration); + cfg.apply( + magnetSensorConfiguration + .withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1) + .withSensorDirection( + inverted + ? SensorDirectionValue.Clockwise_Positive + : SensorDirectionValue.CounterClockwise_Positive)); + } - /** - * Get the absolute position of the encoder. Sets {@link SwerveAbsoluteEncoder#readingError} on erroneous readings. - * - * @return Absolute position in degrees from [0, 360). - */ - @Override - public double getAbsolutePosition() - { - readingError = false; - MagnetHealthValue strength = encoder.getMagnetHealth().getValue(); + /** + * Get the absolute position of the encoder. Sets {@link SwerveAbsoluteEncoder#readingError} on + * erroneous readings. + * + * @return Absolute position in degrees from [0, 360). + */ + @Override + public double getAbsolutePosition() { + readingError = false; + MagnetHealthValue strength = encoder.getMagnetHealth().getValue(); - if (strength != MagnetHealthValue.Magnet_Green) - { - DriverStation.reportWarning( - "CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.\n", false); - } - if (strength == MagnetHealthValue.Magnet_Invalid || strength == MagnetHealthValue.Magnet_Red) - { - readingError = true; - DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " reading was faulty.\n", false); - return 0; - } - StatusSignal angle = encoder.getAbsolutePosition().refresh(); + if (strength != MagnetHealthValue.Magnet_Green) { + DriverStation.reportWarning( + "CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.\n", false); + } + if (strength == MagnetHealthValue.Magnet_Invalid || strength == MagnetHealthValue.Magnet_Red) { + readingError = true; + DriverStation.reportWarning( + "CANCoder " + encoder.getDeviceID() + " reading was faulty.\n", false); + return 0; + } + StatusSignal angle = encoder.getAbsolutePosition().refresh(); - // Taken from democat's library. - // Source: https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/CanCoderFactoryBuilder.java#L51-L74 - for (int i = 0; i < maximumRetries; i++) - { - if (angle.getStatus() == StatusCode.OK) - { - break; - } - angle = angle.waitForUpdate(0.01); - } - if (angle.getStatus() != StatusCode.OK) - { - readingError = true; - DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " reading was faulty, ignoring.\n", false); - } + // Taken from democat's library. + // Source: + // https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/CanCoderFactoryBuilder.java#L51-L74 + for (int i = 0; i < maximumRetries; i++) { + if (angle.getStatus() == StatusCode.OK) { + break; + } + angle = angle.waitForUpdate(0.01); + } + if (angle.getStatus() != StatusCode.OK) { + readingError = true; + DriverStation.reportWarning( + "CANCoder " + encoder.getDeviceID() + " reading was faulty, ignoring.\n", false); + } - return angle.getValue() * 360; - } + return angle.getValue() * 360; + } - /** - * Get the instantiated absolute encoder Object. - * - * @return Absolute encoder object. - */ - @Override - public Object getAbsoluteEncoder() - { - return encoder; - } + /** + * Get the instantiated absolute encoder Object. + * + * @return Absolute encoder object. + */ + @Override + public Object getAbsoluteEncoder() { + return encoder; + } - /** - * Sets the Absolute Encoder Offset within the CANcoder's Memory. - * - * @param offset the offset the Absolute Encoder uses as the zero point in degrees. - * @return if setting Absolute Encoder Offset was successful or not. - */ - @Override - public boolean setAbsoluteEncoderOffset(double offset) - { - CANcoderConfigurator cfg = encoder.getConfigurator(); - MagnetSensorConfigs magCfg = new MagnetSensorConfigs(); - StatusCode error = cfg.refresh(magCfg); - if (error != StatusCode.OK) - { - return false; - } - error = cfg.apply(magCfg.withMagnetOffset(offset / 360)); - if (error == StatusCode.OK) - { - return true; - } - DriverStation.reportWarning( - "Failure to set CANCoder " + encoder.getDeviceID() + " Absolute Encoder Offset Error: " + error, false); - return false; - } + /** + * Sets the Absolute Encoder Offset within the CANcoder's Memory. + * + * @param offset the offset the Absolute Encoder uses as the zero point in degrees. + * @return if setting Absolute Encoder Offset was successful or not. + */ + @Override + public boolean setAbsoluteEncoderOffset(double offset) { + CANcoderConfigurator cfg = encoder.getConfigurator(); + MagnetSensorConfigs magCfg = new MagnetSensorConfigs(); + StatusCode error = cfg.refresh(magCfg); + if (error != StatusCode.OK) { + return false; + } + error = cfg.apply(magCfg.withMagnetOffset(offset / 360)); + if (error == StatusCode.OK) { + return true; + } + DriverStation.reportWarning( + "Failure to set CANCoder " + + encoder.getDeviceID() + + " Absolute Encoder Offset Error: " + + error, + false); + return false; + } - /** - * Get the velocity in degrees/sec. - * - * @return velocity in degrees/sec. - */ - @Override - public double getVelocity() - { - return encoder.getVelocity().getValue() * 360; - } + /** + * Get the velocity in degrees/sec. + * + * @return velocity in degrees/sec. + */ + @Override + public double getVelocity() { + return encoder.getVelocity().getValue() * 360; + } } diff --git a/src/main/java/swervelib/encoders/CanAndCoderSwerve.java b/src/main/java/swervelib/encoders/CanAndCoderSwerve.java index 4b214984..8f1f4b05 100644 --- a/src/main/java/swervelib/encoders/CanAndCoderSwerve.java +++ b/src/main/java/swervelib/encoders/CanAndCoderSwerve.java @@ -3,105 +3,86 @@ import com.reduxrobotics.sensors.canandcoder.Canandcoder; import edu.wpi.first.wpilibj.DriverStation; -/** - * HELIUM {@link Canandcoder} from ReduxRobotics absolute encoder, attached through the CAN bus. - */ -public class CanAndCoderSwerve extends SwerveAbsoluteEncoder -{ +/** HELIUM {@link Canandcoder} from ReduxRobotics absolute encoder, attached through the CAN bus. */ +public class CanAndCoderSwerve extends SwerveAbsoluteEncoder { - /** - * The {@link Canandcoder} representing the CANandCoder on the CAN bus. - */ - public Canandcoder encoder; - /** - * Inversion state of the encoder. - */ - private boolean inverted = false; + /** The {@link Canandcoder} representing the CANandCoder on the CAN bus. */ + public Canandcoder encoder; + /** Inversion state of the encoder. */ + private boolean inverted = false; - /** - * Create the {@link Canandcoder} - * - * @param canid The CAN ID whenever the CANandCoder is operating on the CANBus. - */ - public CanAndCoderSwerve(int canid) - { - encoder = new Canandcoder(canid); - } + /** + * Create the {@link Canandcoder} + * + * @param canid The CAN ID whenever the CANandCoder is operating on the CANBus. + */ + public CanAndCoderSwerve(int canid) { + encoder = new Canandcoder(canid); + } - /** - * Reset the encoder to factory defaults. - */ - @Override - public void factoryDefault() - { - encoder.resetFactoryDefaults(false); - } + /** Reset the encoder to factory defaults. */ + @Override + public void factoryDefault() { + encoder.resetFactoryDefaults(false); + } - /** - * Clear sticky faults on the encoder. - */ - @Override - public void clearStickyFaults() - { - encoder.clearStickyFaults(); - } + /** Clear sticky faults on the encoder. */ + @Override + public void clearStickyFaults() { + encoder.clearStickyFaults(); + } - /** - * Configure the CANandCoder to read from [0, 360) per second. - * - * @param inverted Whether the encoder is inverted. - */ - @Override - public void configure(boolean inverted) - { - this.inverted = inverted; - } + /** + * Configure the CANandCoder to read from [0, 360) per second. + * + * @param inverted Whether the encoder is inverted. + */ + @Override + public void configure(boolean inverted) { + this.inverted = inverted; + } - /** - * Get the absolute position of the encoder. - * - * @return Absolute position in degrees from [0, 360). - */ - @Override - public double getAbsolutePosition() - { - return (inverted ? -1.0 : 1.0) * encoder.getPosition() * 360; - } + /** + * Get the absolute position of the encoder. + * + * @return Absolute position in degrees from [0, 360). + */ + @Override + public double getAbsolutePosition() { + return (inverted ? -1.0 : 1.0) * encoder.getPosition() * 360; + } - /** - * Get the instantiated absolute encoder Object. - * - * @return Absolute encoder object. - */ - @Override - public Object getAbsoluteEncoder() - { - return encoder; - } + /** + * Get the instantiated absolute encoder Object. + * + * @return Absolute encoder object. + */ + @Override + public Object getAbsoluteEncoder() { + return encoder; + } - /** - * Cannot set the offset of the CanAndCoder. - * - * @param offset the offset the Absolute Encoder uses as the zero point. - * @return always false due to CanAndCoder not supporting offset changing. - */ - @Override - public boolean setAbsoluteEncoderOffset(double offset) - { - //CanAndCoder does not support Absolute Offset Changing - DriverStation.reportWarning("Cannot Set Absolute Encoder Offset of CanAndCoders ID: " + encoder.getAddress(), - false); - return false; - } + /** + * Cannot set the offset of the CanAndCoder. + * + * @param offset the offset the Absolute Encoder uses as the zero point. + * @return always false due to CanAndCoder not supporting offset changing. + */ + @Override + public boolean setAbsoluteEncoderOffset(double offset) { + // CanAndCoder does not support Absolute Offset Changing + DriverStation.reportWarning( + "Cannot Set Absolute Encoder Offset of CanAndCoders ID: " + encoder.getAddress(), false); + return false; + } - /** - * Get the velocity in degrees/sec. - * - * @return velocity in degrees/sec. - */ - @Override - public double getVelocity() - { - return encoder.getVelocity(); - } + /** + * Get the velocity in degrees/sec. + * + * @return velocity in degrees/sec. + */ + @Override + public double getVelocity() { + return encoder.getVelocity(); + } } diff --git a/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java b/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java index 0361aec8..fcf0f323 100644 --- a/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java @@ -4,109 +4,93 @@ import edu.wpi.first.wpilibj.DutyCycleEncoder; /** - * DutyCycle encoders such as "US Digital MA3 with PWM Output, the CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag - * Encoder." attached via a PWM lane. - *

- * Credits to - * + * DutyCycle encoders such as "US Digital MA3 with PWM Output, the CTRE Mag Encoder, the Rev Hex + * Encoder, and the AM Mag Encoder." attached via a PWM lane. + * + *

Credits to * p2reneker25 for building this. */ -public class PWMDutyCycleEncoderSwerve extends SwerveAbsoluteEncoder -{ +public class PWMDutyCycleEncoderSwerve extends SwerveAbsoluteEncoder { - /** - * Duty Cycle Encoder. - */ - private final DutyCycleEncoder encoder; - /** - * Inversion state. - */ - private boolean isInverted; + /** Duty Cycle Encoder. */ + private final DutyCycleEncoder encoder; + /** Inversion state. */ + private boolean isInverted; - /** - * Constructor for the PWM duty cycle encoder. - * - * @param pin PWM lane for the encoder. - */ - public PWMDutyCycleEncoderSwerve(int pin) - { - encoder = new DutyCycleEncoder(pin); - } + /** + * Constructor for the PWM duty cycle encoder. + * + * @param pin PWM lane for the encoder. + */ + public PWMDutyCycleEncoderSwerve(int pin) { + encoder = new DutyCycleEncoder(pin); + } - /** - * Configure the inversion state of the encoder. - * - * @param inverted Whether the encoder is inverted. - */ - @Override - public void configure(boolean inverted) - { - isInverted = inverted; - } + /** + * Configure the inversion state of the encoder. + * + * @param inverted Whether the encoder is inverted. + */ + @Override + public void configure(boolean inverted) { + isInverted = inverted; + } - /** - * Get the absolute position of the encoder. - * - * @return Absolute position in degrees from [0, 360). - */ - @Override - public double getAbsolutePosition() - { - return (isInverted ? -1.0 : 1.0) * encoder.getAbsolutePosition() * 360; - } + /** + * Get the absolute position of the encoder. + * + * @return Absolute position in degrees from [0, 360). + */ + @Override + public double getAbsolutePosition() { + return (isInverted ? -1.0 : 1.0) * encoder.getAbsolutePosition() * 360; + } - /** - * Get the encoder object. - * - * @return {@link DutyCycleEncoder} from the class. - */ - @Override - public Object getAbsoluteEncoder() - { - return encoder; - } + /** + * Get the encoder object. + * + * @return {@link DutyCycleEncoder} from the class. + */ + @Override + public Object getAbsoluteEncoder() { + return encoder; + } - /** - * Get the velocity in degrees/sec. - * - * @return velocity in degrees/sec. - */ - @Override - public double getVelocity() - { - DriverStation.reportWarning("The PWM Duty Cycle encoder may not report accurate velocities!", true); - return encoder.get(); - } + /** + * Get the velocity in degrees/sec. + * + * @return velocity in degrees/sec. + */ + @Override + public double getVelocity() { + DriverStation.reportWarning( + "The PWM Duty Cycle encoder may not report accurate velocities!", true); + return encoder.get(); + } - /** - * Reset the encoder to factory defaults. - */ - @Override - public void factoryDefault() - { - // Do nothing - } + /** Reset the encoder to factory defaults. */ + @Override + public void factoryDefault() { + // Do nothing + } - /** - * Clear sticky faults on the encoder. - */ - @Override - public void clearStickyFaults() - { - // Do nothing - } + /** Clear sticky faults on the encoder. */ + @Override + public void clearStickyFaults() { + // Do nothing + } - /** - * Sets the offset of the Encoder in the WPILib Encoder Library. - * - * @param offset the offset the Absolute Encoder uses as the zero point. - * @return Always true due to no external device commands. - */ - @Override - public boolean setAbsoluteEncoderOffset(double offset) - { - encoder.setPositionOffset(offset); + /** + * Sets the offset of the Encoder in the WPILib Encoder Library. + * + * @param offset the offset the Absolute Encoder uses as the zero point. + * @return Always true due to no external device commands. + */ + @Override + public boolean setAbsoluteEncoderOffset(double offset) { + encoder.setPositionOffset(offset); - return true; - } -} \ No newline at end of file + return true; + } +} diff --git a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java index 0508bca8..faefe46d 100644 --- a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java @@ -8,123 +8,104 @@ import java.util.function.Supplier; import swervelib.motors.SwerveMotor; -/** - * SparkMax absolute encoder, attached through the data port analog pin. - */ -public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder -{ +/** SparkMax absolute encoder, attached through the data port analog pin. */ +public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder { - /** - * The {@link SparkAnalogSensor} representing the duty cycle encoder attached to the SparkMax analog port. - */ - public SparkAnalogSensor encoder; + /** + * The {@link SparkAnalogSensor} representing the duty cycle encoder attached to the SparkMax + * analog port. + */ + public SparkAnalogSensor encoder; - /** - * Create the {@link SparkMaxAnalogEncoderSwerve} object as a analog sensor from the {@link CANSparkMax} motor data - * port analog pin. - * - * @param motor Motor to create the encoder from. - */ - public SparkMaxAnalogEncoderSwerve(SwerveMotor motor) - { - if (motor.getMotor() instanceof CANSparkMax) - { - encoder = ((CANSparkMax) motor.getMotor()).getAnalog(Mode.kAbsolute); - } else - { - throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax"); - } - } + /** + * Create the {@link SparkMaxAnalogEncoderSwerve} object as a analog sensor from the {@link + * CANSparkMax} motor data port analog pin. + * + * @param motor Motor to create the encoder from. + */ + public SparkMaxAnalogEncoderSwerve(SwerveMotor motor) { + if (motor.getMotor() instanceof CANSparkMax) { + encoder = ((CANSparkMax) motor.getMotor()).getAnalog(Mode.kAbsolute); + } else { + throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax"); + } + } - /** - * Run the configuration until it succeeds or times out. - * - * @param config Lambda supplier returning the error state. - */ - private void configureSparkMax(Supplier config) - { - for (int i = 0; i < maximumRetries; i++) - { - if (config.get() == REVLibError.kOk) - { - return; - } - } - DriverStation.reportWarning("Failure configuring encoder", true); - } + /** + * Run the configuration until it succeeds or times out. + * + * @param config Lambda supplier returning the error state. + */ + private void configureSparkMax(Supplier config) { + for (int i = 0; i < maximumRetries; i++) { + if (config.get() == REVLibError.kOk) { + return; + } + } + DriverStation.reportWarning("Failure configuring encoder", true); + } - /** - * Reset the encoder to factory defaults. - */ - @Override - public void factoryDefault() - { - // Do nothing - } + /** Reset the encoder to factory defaults. */ + @Override + public void factoryDefault() { + // Do nothing + } - /** - * Clear sticky faults on the encoder. - */ - @Override - public void clearStickyFaults() - { - // Do nothing - } + /** Clear sticky faults on the encoder. */ + @Override + public void clearStickyFaults() { + // Do nothing + } - /** - * Configure the absolute encoder to read from [0, 360) per second. - * - * @param inverted Whether the encoder is inverted. - */ - @Override - public void configure(boolean inverted) - { - encoder.setInverted(inverted); - } + /** + * Configure the absolute encoder to read from [0, 360) per second. + * + * @param inverted Whether the encoder is inverted. + */ + @Override + public void configure(boolean inverted) { + encoder.setInverted(inverted); + } - /** - * Get the absolute position of the encoder. - * - * @return Absolute position in degrees from [0, 360). - */ - @Override - public double getAbsolutePosition() - { - return encoder.getPosition(); - } + /** + * Get the absolute position of the encoder. + * + * @return Absolute position in degrees from [0, 360). + */ + @Override + public double getAbsolutePosition() { + return encoder.getPosition(); + } - /** - * Get the instantiated absolute encoder Object. - * - * @return Absolute encoder object. - */ - @Override - public Object getAbsoluteEncoder() - { - return encoder; - } + /** + * Get the instantiated absolute encoder Object. + * + * @return Absolute encoder object. + */ + @Override + public Object getAbsoluteEncoder() { + return encoder; + } - /** - * Sets the Absolute Encoder offset at the Encoder Level. - * - * @param offset the offset the Absolute Encoder uses as the zero point. - * @return if setting Absolute Encoder Offset was successful or not. - */ - @Override - public boolean setAbsoluteEncoderOffset(double offset) - { - DriverStation.reportWarning("SparkMax Analog Sensor's do not support integrated offsets", true); - return false; - } + /** + * Sets the Absolute Encoder offset at the Encoder Level. + * + * @param offset the offset the Absolute Encoder uses as the zero point. + * @return if setting Absolute Encoder Offset was successful or not. + */ + @Override + public boolean setAbsoluteEncoderOffset(double offset) { + DriverStation.reportWarning("SparkMax Analog Sensor's do not support integrated offsets", true); + return false; + } - /** - * Get the velocity in degrees/sec. - * - * @return velocity in degrees/sec. - */ - @Override - public double getVelocity() - { - return encoder.getVelocity(); - } + /** + * Get the velocity in degrees/sec. + * + * @return velocity in degrees/sec. + */ + @Override + public double getVelocity() { + return encoder.getVelocity(); + } } diff --git a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java index f95d36d2..344c08e6 100644 --- a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java @@ -8,134 +8,111 @@ import java.util.function.Supplier; import swervelib.motors.SwerveMotor; -/** - * SparkMax absolute encoder, attached through the data port. - */ -public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder -{ +/** SparkMax absolute encoder, attached through the data port. */ +public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder { - /** - * The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkMax. - */ - public AbsoluteEncoder encoder; + /** The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkMax. */ + public AbsoluteEncoder encoder; - /** - * Create the {@link SparkMaxEncoderSwerve} object as a duty cycle from the {@link CANSparkMax} motor. - * - * @param motor Motor to create the encoder from. - * @param conversionFactor The conversion factor to set if the output is not from 0 to 360. - */ - public SparkMaxEncoderSwerve(SwerveMotor motor, int conversionFactor) - { - if (motor.getMotor() instanceof CANSparkMax) - { - encoder = ((CANSparkMax) motor.getMotor()).getAbsoluteEncoder(Type.kDutyCycle); - configureSparkMax(() -> encoder.setVelocityConversionFactor(conversionFactor)); - configureSparkMax(() -> encoder.setPositionConversionFactor(conversionFactor)); - } else - { - throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax"); - } - } + /** + * Create the {@link SparkMaxEncoderSwerve} object as a duty cycle from the {@link CANSparkMax} + * motor. + * + * @param motor Motor to create the encoder from. + * @param conversionFactor The conversion factor to set if the output is not from 0 to 360. + */ + public SparkMaxEncoderSwerve(SwerveMotor motor, int conversionFactor) { + if (motor.getMotor() instanceof CANSparkMax) { + encoder = ((CANSparkMax) motor.getMotor()).getAbsoluteEncoder(Type.kDutyCycle); + configureSparkMax(() -> encoder.setVelocityConversionFactor(conversionFactor)); + configureSparkMax(() -> encoder.setPositionConversionFactor(conversionFactor)); + } else { + throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax"); + } + } - /** - * Run the configuration until it succeeds or times out. - * - * @param config Lambda supplier returning the error state. - */ - private void configureSparkMax(Supplier config) - { - for (int i = 0; i < maximumRetries; i++) - { - if (config.get() == REVLibError.kOk) - { - return; - } - } - DriverStation.reportWarning("Failure configuring encoder", true); - } + /** + * Run the configuration until it succeeds or times out. + * + * @param config Lambda supplier returning the error state. + */ + private void configureSparkMax(Supplier config) { + for (int i = 0; i < maximumRetries; i++) { + if (config.get() == REVLibError.kOk) { + return; + } + } + DriverStation.reportWarning("Failure configuring encoder", true); + } - /** - * Reset the encoder to factory defaults. - */ - @Override - public void factoryDefault() - { - // Do nothing - } + /** Reset the encoder to factory defaults. */ + @Override + public void factoryDefault() { + // Do nothing + } - /** - * Clear sticky faults on the encoder. - */ - @Override - public void clearStickyFaults() - { - // Do nothing - } + /** Clear sticky faults on the encoder. */ + @Override + public void clearStickyFaults() { + // Do nothing + } - /** - * Configure the absolute encoder to read from [0, 360) per second. - * - * @param inverted Whether the encoder is inverted. - */ - @Override - public void configure(boolean inverted) - { - encoder.setInverted(inverted); - } + /** + * Configure the absolute encoder to read from [0, 360) per second. + * + * @param inverted Whether the encoder is inverted. + */ + @Override + public void configure(boolean inverted) { + encoder.setInverted(inverted); + } - /** - * Get the absolute position of the encoder. - * - * @return Absolute position in degrees from [0, 360). - */ - @Override - public double getAbsolutePosition() - { - return encoder.getPosition(); - } + /** + * Get the absolute position of the encoder. + * + * @return Absolute position in degrees from [0, 360). + */ + @Override + public double getAbsolutePosition() { + return encoder.getPosition(); + } - /** - * Get the instantiated absolute encoder Object. - * - * @return Absolute encoder object. - */ - @Override - public Object getAbsoluteEncoder() - { - return encoder; - } + /** + * Get the instantiated absolute encoder Object. + * + * @return Absolute encoder object. + */ + @Override + public Object getAbsoluteEncoder() { + return encoder; + } - /** - * Sets the Absolute Encoder Offset inside of the SparkMax's Memory. - * - * @param offset the offset the Absolute Encoder uses as the zero point. - * @return if setting Absolute Encoder Offset was successful or not. - */ - @Override - public boolean setAbsoluteEncoderOffset(double offset) - { - REVLibError error = null; - for (int i = 0; i < maximumRetries; i++) - { - error = encoder.setZeroOffset(offset); - if (error == REVLibError.kOk) - { - return true; - } - } - DriverStation.reportWarning("Failure to set Absolute Encoder Offset Error: " + error, false); - return false; - } + /** + * Sets the Absolute Encoder Offset inside of the SparkMax's Memory. + * + * @param offset the offset the Absolute Encoder uses as the zero point. + * @return if setting Absolute Encoder Offset was successful or not. + */ + @Override + public boolean setAbsoluteEncoderOffset(double offset) { + REVLibError error = null; + for (int i = 0; i < maximumRetries; i++) { + error = encoder.setZeroOffset(offset); + if (error == REVLibError.kOk) { + return true; + } + } + DriverStation.reportWarning("Failure to set Absolute Encoder Offset Error: " + error, false); + return false; + } - /** - * Get the velocity in degrees/sec. - * - * @return velocity in degrees/sec. - */ - @Override - public double getVelocity() - { - return encoder.getVelocity(); - } + /** + * Get the velocity in degrees/sec. + * + * @return velocity in degrees/sec. + */ + @Override + public double getVelocity() { + return encoder.getVelocity(); + } } diff --git a/src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java b/src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java index a01e4c47..9c95e1b2 100644 --- a/src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java +++ b/src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java @@ -1,62 +1,58 @@ package swervelib.encoders; /** - * Swerve abstraction class to define a standard interface with absolute encoders for swerve modules.. + * Swerve abstraction class to define a standard interface with absolute encoders for swerve + * modules.. */ -public abstract class SwerveAbsoluteEncoder -{ - - /** - * The maximum amount of times the swerve encoder will attempt to configure itself if failures occur. - */ - public final int maximumRetries = 5; - /** - * Last angle reading was faulty. - */ - public boolean readingError = false; - - /** - * Reset the encoder to factory defaults. - */ - public abstract void factoryDefault(); - - /** - * Clear sticky faults on the encoder. - */ - public abstract void clearStickyFaults(); - - /** - * Configure the absolute encoder to read from [0, 360) per second. - * - * @param inverted Whether the encoder is inverted. - */ - public abstract void configure(boolean inverted); - - /** - * Get the absolute position of the encoder. - * - * @return Absolute position in degrees from [0, 360). - */ - public abstract double getAbsolutePosition(); - - /** - * Get the instantiated absolute encoder Object. - * - * @return Absolute encoder object. - */ - public abstract Object getAbsoluteEncoder(); - - /** - * Sets the Absolute Encoder offset at the Encoder Level. - * - * @param offset the offset the Absolute Encoder uses as the zero point. - * @return if setting Absolute Encoder Offset was successful or not. - */ - public abstract boolean setAbsoluteEncoderOffset(double offset); - - /** - * Get the velocity in degrees/sec. - * @return velocity in degrees/sec. - */ - public abstract double getVelocity(); +public abstract class SwerveAbsoluteEncoder { + + /** + * The maximum amount of times the swerve encoder will attempt to configure itself if failures + * occur. + */ + public final int maximumRetries = 5; + /** Last angle reading was faulty. */ + public boolean readingError = false; + + /** Reset the encoder to factory defaults. */ + public abstract void factoryDefault(); + + /** Clear sticky faults on the encoder. */ + public abstract void clearStickyFaults(); + + /** + * Configure the absolute encoder to read from [0, 360) per second. + * + * @param inverted Whether the encoder is inverted. + */ + public abstract void configure(boolean inverted); + + /** + * Get the absolute position of the encoder. + * + * @return Absolute position in degrees from [0, 360). + */ + public abstract double getAbsolutePosition(); + + /** + * Get the instantiated absolute encoder Object. + * + * @return Absolute encoder object. + */ + public abstract Object getAbsoluteEncoder(); + + /** + * Sets the Absolute Encoder offset at the Encoder Level. + * + * @param offset the offset the Absolute Encoder uses as the zero point. + * @return if setting Absolute Encoder Offset was successful or not. + */ + public abstract boolean setAbsoluteEncoderOffset(double offset); + + /** + * Get the velocity in degrees/sec. + * + * @return velocity in degrees/sec. + */ + public abstract double getVelocity(); } diff --git a/src/main/java/swervelib/encoders/package-info.java b/src/main/java/swervelib/encoders/package-info.java index ab968083..6b93a2d5 100644 --- a/src/main/java/swervelib/encoders/package-info.java +++ b/src/main/java/swervelib/encoders/package-info.java @@ -1,4 +1,5 @@ /** - * Absolute encoders for the swerve drive, all implement {@link swervelib.encoders.SwerveAbsoluteEncoder}. + * Absolute encoders for the swerve drive, all implement {@link + * swervelib.encoders.SwerveAbsoluteEncoder}. */ package swervelib.encoders; diff --git a/src/main/java/swervelib/imu/ADIS16448Swerve.java b/src/main/java/swervelib/imu/ADIS16448Swerve.java index d60f8c6a..0e340fcc 100644 --- a/src/main/java/swervelib/imu/ADIS16448Swerve.java +++ b/src/main/java/swervelib/imu/ADIS16448Swerve.java @@ -6,103 +6,86 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.Optional; -/** - * IMU Swerve class for the {@link ADIS16448_IMU} device. - */ -public class ADIS16448Swerve extends SwerveIMU -{ +/** IMU Swerve class for the {@link ADIS16448_IMU} device. */ +public class ADIS16448Swerve extends SwerveIMU { - /** - * {@link ADIS16448_IMU} device to read the current headings from. - */ - private final ADIS16448_IMU imu; - /** - * Offset for the ADIS16448. - */ - private Rotation3d offset = new Rotation3d(); + /** {@link ADIS16448_IMU} device to read the current headings from. */ + private final ADIS16448_IMU imu; + /** Offset for the ADIS16448. */ + private Rotation3d offset = new Rotation3d(); - /** - * Construct the ADIS16448 imu and reset default configurations. Publish the gyro to the SmartDashboard. - */ - public ADIS16448Swerve() - { - imu = new ADIS16448_IMU(); - factoryDefault(); - SmartDashboard.putData(imu); - } + /** + * Construct the ADIS16448 imu and reset default configurations. Publish the gyro to the + * SmartDashboard. + */ + public ADIS16448Swerve() { + imu = new ADIS16448_IMU(); + factoryDefault(); + SmartDashboard.putData(imu); + } - /** - * Reset IMU to factory default. - */ - @Override - public void factoryDefault() - { - offset = new Rotation3d(0, 0, 0); - imu.calibrate(); - } + /** Reset IMU to factory default. */ + @Override + public void factoryDefault() { + offset = new Rotation3d(0, 0, 0); + imu.calibrate(); + } - /** - * Clear sticky faults on IMU. - */ - @Override - public void clearStickyFaults() - { - // Do nothing. - } + /** Clear sticky faults on IMU. */ + @Override + public void clearStickyFaults() { + // Do nothing. + } - /** - * Set the gyro offset. - * - * @param offset gyro offset as a {@link Rotation3d}. - */ - public void setOffset(Rotation3d offset) - { - this.offset = offset; - } + /** + * Set the gyro offset. + * + * @param offset gyro offset as a {@link Rotation3d}. + */ + public void setOffset(Rotation3d offset) { + this.offset = offset; + } - /** - * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - public Rotation3d getRawRotation3d() - { - return new Rotation3d(Math.toRadians(-imu.getGyroAngleX()), - Math.toRadians(-imu.getGyroAngleY()), - Math.toRadians(-imu.getGyroAngleZ())); - } + /** + * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + public Rotation3d getRawRotation3d() { + return new Rotation3d( + Math.toRadians(-imu.getGyroAngleX()), + Math.toRadians(-imu.getGyroAngleY()), + Math.toRadians(-imu.getGyroAngleZ())); + } - /** - * Fetch the {@link Rotation3d} from the IMU. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - @Override - public Rotation3d getRotation3d() - { - return getRawRotation3d().minus(offset); - } + /** + * Fetch the {@link Rotation3d} from the IMU. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + @Override + public Rotation3d getRotation3d() { + return getRawRotation3d().minus(offset); + } - /** - * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns - * empty. - * - * @return {@link Translation3d} of the acceleration. - */ - @Override - public Optional getAccel() - { - return Optional.of(new Translation3d(imu.getAccelX(), imu.getAccelY(), imu.getAccelZ())); - } + /** + * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration + * isn't supported returns empty. + * + * @return {@link Translation3d} of the acceleration. + */ + @Override + public Optional getAccel() { + return Optional.of(new Translation3d(imu.getAccelX(), imu.getAccelY(), imu.getAccelZ())); + } - /** - * Get the instantiated IMU object. - * - * @return IMU object. - */ - @Override - public Object getIMU() - { - return imu; - } + /** + * Get the instantiated IMU object. + * + * @return IMU object. + */ + @Override + public Object getIMU() { + return imu; + } } diff --git a/src/main/java/swervelib/imu/ADIS16470Swerve.java b/src/main/java/swervelib/imu/ADIS16470Swerve.java index 5a102bf0..4bbc1884 100644 --- a/src/main/java/swervelib/imu/ADIS16470Swerve.java +++ b/src/main/java/swervelib/imu/ADIS16470Swerve.java @@ -2,107 +2,89 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis; import edu.wpi.first.wpilibj.ADIS16470_IMU; +import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.Optional; -/** - * IMU Swerve class for the {@link ADIS16470_IMU} device. - */ -public class ADIS16470Swerve extends SwerveIMU -{ +/** IMU Swerve class for the {@link ADIS16470_IMU} device. */ +public class ADIS16470Swerve extends SwerveIMU { - /** - * {@link ADIS16470_IMU} device to read the current headings from. - */ - private final ADIS16470_IMU imu; - /** - * Offset for the ADIS16470. - */ - private Rotation3d offset = new Rotation3d(); + /** {@link ADIS16470_IMU} device to read the current headings from. */ + private final ADIS16470_IMU imu; + /** Offset for the ADIS16470. */ + private Rotation3d offset = new Rotation3d(); - /** - * Construct the ADIS16470 imu and reset default configurations. Publish the gyro to the SmartDashboard. - */ - public ADIS16470Swerve() - { - imu = new ADIS16470_IMU(); - offset = new Rotation3d(); - factoryDefault(); - SmartDashboard.putData(imu); - } + /** + * Construct the ADIS16470 imu and reset default configurations. Publish the gyro to the + * SmartDashboard. + */ + public ADIS16470Swerve() { + imu = new ADIS16470_IMU(); + offset = new Rotation3d(); + factoryDefault(); + SmartDashboard.putData(imu); + } - /** - * Reset IMU to factory default. - */ - @Override - public void factoryDefault() - { - offset = new Rotation3d(0, 0, 0); - imu.calibrate(); - } + /** Reset IMU to factory default. */ + @Override + public void factoryDefault() { + offset = new Rotation3d(0, 0, 0); + imu.calibrate(); + } - /** - * Clear sticky faults on IMU. - */ - @Override - public void clearStickyFaults() - { - // Do nothing. - } + /** Clear sticky faults on IMU. */ + @Override + public void clearStickyFaults() { + // Do nothing. + } - /** - * Set the gyro offset. - * - * @param offset gyro offset as a {@link Rotation3d}. - */ - public void setOffset(Rotation3d offset) - { - this.offset = offset; - } + /** + * Set the gyro offset. + * + * @param offset gyro offset as a {@link Rotation3d}. + */ + public void setOffset(Rotation3d offset) { + this.offset = offset; + } - /** - * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - public Rotation3d getRawRotation3d() - { - return new Rotation3d(0, 0, Math.toRadians(-imu.getAngle(IMUAxis.kYaw))); - } + /** + * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + public Rotation3d getRawRotation3d() { + return new Rotation3d(0, 0, Math.toRadians(-imu.getAngle(IMUAxis.kYaw))); + } - /** - * Fetch the {@link Rotation3d} from the IMU. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - @Override - public Rotation3d getRotation3d() - { - return getRawRotation3d().minus(offset); - } + /** + * Fetch the {@link Rotation3d} from the IMU. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + @Override + public Rotation3d getRotation3d() { + return getRawRotation3d().minus(offset); + } - /** - * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns - * empty. - * - * @return {@link Translation3d} of the acceleration as an {@link Optional}. - */ - @Override - public Optional getAccel() - { - return Optional.of(new Translation3d(imu.getAccelX(), imu.getAccelY(), imu.getAccelZ())); - } + /** + * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration + * isn't supported returns empty. + * + * @return {@link Translation3d} of the acceleration as an {@link Optional}. + */ + @Override + public Optional getAccel() { + return Optional.of(new Translation3d(imu.getAccelX(), imu.getAccelY(), imu.getAccelZ())); + } - /** - * Get the instantiated IMU object. - * - * @return IMU object. - */ - @Override - public Object getIMU() - { - return imu; - } + /** + * Get the instantiated IMU object. + * + * @return IMU object. + */ + @Override + public Object getIMU() { + return imu; + } } diff --git a/src/main/java/swervelib/imu/ADXRS450Swerve.java b/src/main/java/swervelib/imu/ADXRS450Swerve.java index f313fb0d..82611572 100644 --- a/src/main/java/swervelib/imu/ADXRS450Swerve.java +++ b/src/main/java/swervelib/imu/ADXRS450Swerve.java @@ -6,101 +6,83 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.Optional; -/** - * IMU Swerve class for the {@link ADXRS450_Gyro} device. - */ -public class ADXRS450Swerve extends SwerveIMU -{ +/** IMU Swerve class for the {@link ADXRS450_Gyro} device. */ +public class ADXRS450Swerve extends SwerveIMU { - /** - * {@link ADXRS450_Gyro} device to read the current headings from. - */ - private final ADXRS450_Gyro imu; - /** - * Offset for the ADXRS450. - */ - private Rotation3d offset = new Rotation3d(); + /** {@link ADXRS450_Gyro} device to read the current headings from. */ + private final ADXRS450_Gyro imu; + /** Offset for the ADXRS450. */ + private Rotation3d offset = new Rotation3d(); - /** - * Construct the ADXRS450 imu and reset default configurations. Publish the gyro to the SmartDashboard. - */ - public ADXRS450Swerve() - { - imu = new ADXRS450_Gyro(); - factoryDefault(); - SmartDashboard.putData(imu); - } + /** + * Construct the ADXRS450 imu and reset default configurations. Publish the gyro to the + * SmartDashboard. + */ + public ADXRS450Swerve() { + imu = new ADXRS450_Gyro(); + factoryDefault(); + SmartDashboard.putData(imu); + } - /** - * Reset IMU to factory default. - */ - @Override - public void factoryDefault() - { - imu.calibrate(); - offset = new Rotation3d(0, 0, 0);//Math.toRadians(-imu.getAngle())); - } + /** Reset IMU to factory default. */ + @Override + public void factoryDefault() { + imu.calibrate(); + offset = new Rotation3d(0, 0, 0); // Math.toRadians(-imu.getAngle())); + } - /** - * Clear sticky faults on IMU. - */ - @Override - public void clearStickyFaults() - { - // Do nothing. - } + /** Clear sticky faults on IMU. */ + @Override + public void clearStickyFaults() { + // Do nothing. + } - /** - * Set the gyro offset. - * - * @param offset gyro offset as a {@link Rotation3d}. - */ - public void setOffset(Rotation3d offset) - { - this.offset = offset; - } + /** + * Set the gyro offset. + * + * @param offset gyro offset as a {@link Rotation3d}. + */ + public void setOffset(Rotation3d offset) { + this.offset = offset; + } - /** - * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - public Rotation3d getRawRotation3d() - { - return new Rotation3d(0, 0, Math.toRadians(-imu.getAngle())); - } + /** + * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + public Rotation3d getRawRotation3d() { + return new Rotation3d(0, 0, Math.toRadians(-imu.getAngle())); + } - /** - * Fetch the {@link Rotation3d} from the IMU. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - @Override - public Rotation3d getRotation3d() - { - return getRawRotation3d().minus(offset); - } + /** + * Fetch the {@link Rotation3d} from the IMU. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + @Override + public Rotation3d getRotation3d() { + return getRawRotation3d().minus(offset); + } - /** - * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns - * empty. - * - * @return {@link Translation3d} of the acceleration as an {@link Optional}. - */ - @Override - public Optional getAccel() - { - return Optional.empty(); - } + /** + * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration + * isn't supported returns empty. + * + * @return {@link Translation3d} of the acceleration as an {@link Optional}. + */ + @Override + public Optional getAccel() { + return Optional.empty(); + } - /** - * Get the instantiated IMU object. - * - * @return IMU object. - */ - @Override - public Object getIMU() - { - return imu; - } + /** + * Get the instantiated IMU object. + * + * @return IMU object. + */ + @Override + public Object getIMU() { + return imu; + } } diff --git a/src/main/java/swervelib/imu/AnalogGyroSwerve.java b/src/main/java/swervelib/imu/AnalogGyroSwerve.java index 590666f7..0d847f3f 100644 --- a/src/main/java/swervelib/imu/AnalogGyroSwerve.java +++ b/src/main/java/swervelib/imu/AnalogGyroSwerve.java @@ -6,108 +6,88 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.Optional; -/** - * Creates a IMU for {@link edu.wpi.first.wpilibj.AnalogGyro} devices, only uses yaw. - */ -public class AnalogGyroSwerve extends SwerveIMU -{ +/** Creates a IMU for {@link edu.wpi.first.wpilibj.AnalogGyro} devices, only uses yaw. */ +public class AnalogGyroSwerve extends SwerveIMU { - /** - * Gyroscope object. - */ - private final AnalogGyro gyro; - /** - * Offset for the analog gyro. - */ - private Rotation3d offset = new Rotation3d(); + /** Gyroscope object. */ + private final AnalogGyro gyro; + /** Offset for the analog gyro. */ + private Rotation3d offset = new Rotation3d(); - /** - * Analog port in which the gyroscope is connected. Can only be attached to analog ports 0 or 1. - * - * @param channel Analog port 0 or 1. - */ - public AnalogGyroSwerve(int channel) - { - if (!(channel == 0 || channel == 1)) - { - throw new RuntimeException( - "Analog Gyroscope must be attached to port 0 or 1 on the roboRIO.\n"); - } - gyro = new AnalogGyro(channel); - factoryDefault(); - SmartDashboard.putData(gyro); - } + /** + * Analog port in which the gyroscope is connected. Can only be attached to analog ports 0 or 1. + * + * @param channel Analog port 0 or 1. + */ + public AnalogGyroSwerve(int channel) { + if (!(channel == 0 || channel == 1)) { + throw new RuntimeException( + "Analog Gyroscope must be attached to port 0 or 1 on the roboRIO.\n"); + } + gyro = new AnalogGyro(channel); + factoryDefault(); + SmartDashboard.putData(gyro); + } - /** - * Reset IMU to factory default. - */ - @Override - public void factoryDefault() - { - gyro.calibrate(); - offset = new Rotation3d(0, 0, 0); - } + /** Reset IMU to factory default. */ + @Override + public void factoryDefault() { + gyro.calibrate(); + offset = new Rotation3d(0, 0, 0); + } - /** - * Clear sticky faults on IMU. - */ - @Override - public void clearStickyFaults() - { - // Do nothing. - } + /** Clear sticky faults on IMU. */ + @Override + public void clearStickyFaults() { + // Do nothing. + } - /** - * Set the gyro offset. - * - * @param offset gyro offset as a {@link Rotation3d}. - */ - public void setOffset(Rotation3d offset) - { - this.offset = offset; - } + /** + * Set the gyro offset. + * + * @param offset gyro offset as a {@link Rotation3d}. + */ + public void setOffset(Rotation3d offset) { + this.offset = offset; + } - /** - * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - public Rotation3d getRawRotation3d() - { - return new Rotation3d(0, 0, Math.toRadians(-gyro.getAngle())); - } + /** + * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + public Rotation3d getRawRotation3d() { + return new Rotation3d(0, 0, Math.toRadians(-gyro.getAngle())); + } - /** - * Fetch the {@link Rotation3d} from the IMU. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - @Override - public Rotation3d getRotation3d() - { - return getRawRotation3d().minus(offset); - } + /** + * Fetch the {@link Rotation3d} from the IMU. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + @Override + public Rotation3d getRotation3d() { + return getRawRotation3d().minus(offset); + } - /** - * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns - * empty. - * - * @return {@link Translation3d} of the acceleration as an {@link Optional}. - */ - @Override - public Optional getAccel() - { - return Optional.empty(); - } + /** + * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration + * isn't supported returns empty. + * + * @return {@link Translation3d} of the acceleration as an {@link Optional}. + */ + @Override + public Optional getAccel() { + return Optional.empty(); + } - /** - * Get the instantiated IMU object. - * - * @return IMU object. - */ - @Override - public Object getIMU() - { - return gyro; - } + /** + * Get the instantiated IMU object. + * + * @return IMU object. + */ + @Override + public Object getIMU() { + return gyro; + } } diff --git a/src/main/java/swervelib/imu/NavXSwerve.java b/src/main/java/swervelib/imu/NavXSwerve.java index 272efafa..b87ca629 100644 --- a/src/main/java/swervelib/imu/NavXSwerve.java +++ b/src/main/java/swervelib/imu/NavXSwerve.java @@ -1,7 +1,6 @@ package swervelib.imu; import com.kauailabs.navx.frc.AHRS; -import edu.wpi.first.math.geometry.Quaternion; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.wpilibj.DriverStation; @@ -11,159 +10,131 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.Optional; -/** - * Communicates with the NavX as the IMU. - */ -public class NavXSwerve extends SwerveIMU -{ +/** Communicates with the NavX as the IMU. */ +public class NavXSwerve extends SwerveIMU { - /** - * NavX IMU. - */ - private AHRS gyro; - /** - * Offset for the NavX. - */ - private Rotation3d offset = new Rotation3d(); + /** NavX IMU. */ + private AHRS gyro; + /** Offset for the NavX. */ + private Rotation3d offset = new Rotation3d(); - /** - * Constructor for the NavX swerve. - * - * @param port Serial Port to connect to. - */ - public NavXSwerve(SerialPort.Port port) - { - try - { - /* Communicate w/navX-MXP via the MXP SPI Bus. */ - /* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */ - /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */ - gyro = new AHRS(port); - factoryDefault(); - SmartDashboard.putData(gyro); - } catch (RuntimeException ex) - { - DriverStation.reportError("Error instantiating navX: " + ex.getMessage(), true); - } - } + /** + * Constructor for the NavX swerve. + * + * @param port Serial Port to connect to. + */ + public NavXSwerve(SerialPort.Port port) { + try { + /* Communicate w/navX-MXP via the MXP SPI Bus. */ + /* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */ + /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */ + gyro = new AHRS(port); + factoryDefault(); + SmartDashboard.putData(gyro); + } catch (RuntimeException ex) { + DriverStation.reportError("Error instantiating navX: " + ex.getMessage(), true); + } + } - /** - * Constructor for the NavX swerve. - * - * @param port SPI Port to connect to. - */ - public NavXSwerve(SPI.Port port) - { - try - { - /* Communicate w/navX-MXP via the MXP SPI Bus. */ - /* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */ - /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */ - gyro = new AHRS(port); - factoryDefault(); - SmartDashboard.putData(gyro); - } catch (RuntimeException ex) - { - DriverStation.reportError("Error instantiating navX: " + ex.getMessage(), true); - } - } + /** + * Constructor for the NavX swerve. + * + * @param port SPI Port to connect to. + */ + public NavXSwerve(SPI.Port port) { + try { + /* Communicate w/navX-MXP via the MXP SPI Bus. */ + /* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */ + /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */ + gyro = new AHRS(port); + factoryDefault(); + SmartDashboard.putData(gyro); + } catch (RuntimeException ex) { + DriverStation.reportError("Error instantiating navX: " + ex.getMessage(), true); + } + } - /** - * Constructor for the NavX swerve. - * - * @param port I2C Port to connect to. - */ - public NavXSwerve(I2C.Port port) - { - try - { - /* Communicate w/navX-MXP via the MXP SPI Bus. */ - /* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */ - /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */ - gyro = new AHRS(port); - factoryDefault(); - SmartDashboard.putData(gyro); - } catch (RuntimeException ex) - { - DriverStation.reportError("Error instantiating navX: " + ex.getMessage(), true); - } - } + /** + * Constructor for the NavX swerve. + * + * @param port I2C Port to connect to. + */ + public NavXSwerve(I2C.Port port) { + try { + /* Communicate w/navX-MXP via the MXP SPI Bus. */ + /* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */ + /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */ + gyro = new AHRS(port); + factoryDefault(); + SmartDashboard.putData(gyro); + } catch (RuntimeException ex) { + DriverStation.reportError("Error instantiating navX: " + ex.getMessage(), true); + } + } - /** - * Reset IMU to factory default. - */ - @Override - public void factoryDefault() - { - // gyro.reset(); // Reported to be slow - offset = gyro.getRotation3d(); - } + /** Reset IMU to factory default. */ + @Override + public void factoryDefault() { + // gyro.reset(); // Reported to be slow + offset = gyro.getRotation3d(); + } - /** - * Clear sticky faults on IMU. - */ - @Override - public void clearStickyFaults() - { - } + /** Clear sticky faults on IMU. */ + @Override + public void clearStickyFaults() {} - /** - * Set the gyro offset. - * - * @param offset gyro offset as a {@link Rotation3d}. - */ - public void setOffset(Rotation3d offset) - { - this.offset = offset; - } + /** + * Set the gyro offset. + * + * @param offset gyro offset as a {@link Rotation3d}. + */ + public void setOffset(Rotation3d offset) { + this.offset = offset; + } - /** - * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - @Override - public Rotation3d getRawRotation3d() - { - return gyro.getRotation3d(); - } + /** + * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + @Override + public Rotation3d getRawRotation3d() { + return gyro.getRotation3d(); + } - /** - * Fetch the {@link Rotation3d} from the IMU. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - @Override - public Rotation3d getRotation3d() - { - return gyro.getRotation3d().minus(offset); - } + /** + * Fetch the {@link Rotation3d} from the IMU. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + @Override + public Rotation3d getRotation3d() { + return gyro.getRotation3d().minus(offset); + } - /** - * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns - * empty. - * - * @return {@link Translation3d} of the acceleration as an {@link Optional}. - */ - @Override - public Optional getAccel() - { - return Optional.of( - new Translation3d( - gyro.getWorldLinearAccelX(), - gyro.getWorldLinearAccelY(), - gyro.getWorldLinearAccelZ()) - .times(9.81)); - } + /** + * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration + * isn't supported returns empty. + * + * @return {@link Translation3d} of the acceleration as an {@link Optional}. + */ + @Override + public Optional getAccel() { + return Optional.of( + new Translation3d( + gyro.getWorldLinearAccelX(), + gyro.getWorldLinearAccelY(), + gyro.getWorldLinearAccelZ()) + .times(9.81)); + } - /** - * Get the instantiated IMU object. - * - * @return IMU object. - */ - @Override - public Object getIMU() - { - return gyro; - } + /** + * Get the instantiated IMU object. + * + * @return IMU object. + */ + @Override + public Object getIMU() { + return gyro; + } } diff --git a/src/main/java/swervelib/imu/Pigeon2Swerve.java b/src/main/java/swervelib/imu/Pigeon2Swerve.java index 61fb6f83..4a99237a 100644 --- a/src/main/java/swervelib/imu/Pigeon2Swerve.java +++ b/src/main/java/swervelib/imu/Pigeon2Swerve.java @@ -10,127 +10,115 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.Optional; -/** - * SwerveIMU interface for the Pigeon2 - */ -public class Pigeon2Swerve extends SwerveIMU -{ +/** SwerveIMU interface for the Pigeon2 */ +public class Pigeon2Swerve extends SwerveIMU { - /** - * Pigeon2 IMU device. - */ - Pigeon2 imu; - /** - * Offset for the Pigeon 2. - */ - private Rotation3d offset = new Rotation3d(); + /** Pigeon2 IMU device. */ + Pigeon2 imu; + /** Offset for the Pigeon 2. */ + private Rotation3d offset = new Rotation3d(); - /** - * Generate the SwerveIMU for pigeon. - * - * @param canid CAN ID for the pigeon - * @param canbus CAN Bus name the pigeon resides on. - */ - public Pigeon2Swerve(int canid, String canbus) - { - imu = new Pigeon2(canid, canbus); - SmartDashboard.putData(imu); - } + /** + * Generate the SwerveIMU for pigeon. + * + * @param canid CAN ID for the pigeon + * @param canbus CAN Bus name the pigeon resides on. + */ + public Pigeon2Swerve(int canid, String canbus) { + imu = new Pigeon2(canid, canbus); + SmartDashboard.putData(imu); + } - /** - * Generate the SwerveIMU for pigeon. - * - * @param canid CAN ID for the pigeon - */ - public Pigeon2Swerve(int canid) - { - this(canid, ""); - } + /** + * Generate the SwerveIMU for pigeon. + * + * @param canid CAN ID for the pigeon + */ + public Pigeon2Swerve(int canid) { + this(canid, ""); + } - /** - * Reset IMU to factory default. - */ - @Override - public void factoryDefault() - { - Pigeon2Configurator cfg = imu.getConfigurator(); - Pigeon2Configuration config = new Pigeon2Configuration(); + /** Reset IMU to factory default. */ + @Override + public void factoryDefault() { + Pigeon2Configurator cfg = imu.getConfigurator(); + Pigeon2Configuration config = new Pigeon2Configuration(); - // Compass utilization causes readings to jump dramatically in some cases. - cfg.apply(config.Pigeon2Features.withEnableCompass(false)); - } + // Compass utilization causes readings to jump dramatically in some cases. + cfg.apply(config.Pigeon2Features.withEnableCompass(false)); + } - /** - * Clear sticky faults on IMU. - */ - @Override - public void clearStickyFaults() - { - imu.clearStickyFaults(); - } + /** Clear sticky faults on IMU. */ + @Override + public void clearStickyFaults() { + imu.clearStickyFaults(); + } - /** - * Set the gyro offset. - * - * @param offset gyro offset as a {@link Rotation3d}. - */ - public void setOffset(Rotation3d offset) - { - this.offset = offset; - } + /** + * Set the gyro offset. + * + * @param offset gyro offset as a {@link Rotation3d}. + */ + public void setOffset(Rotation3d offset) { + this.offset = offset; + } - /** - * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - @Override - public Rotation3d getRawRotation3d() - { - // TODO: Switch to suppliers. - StatusSignal w = imu.getQuatW(); - StatusSignal x = imu.getQuatX(); - StatusSignal y = imu.getQuatY(); - StatusSignal z = imu.getQuatZ(); - return new Rotation3d(new Quaternion(w.refresh().getValue(), x.refresh().getValue(), y.refresh().getValue(), z.refresh().getValue())); - } + /** + * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + @Override + public Rotation3d getRawRotation3d() { + // TODO: Switch to suppliers. + StatusSignal w = imu.getQuatW(); + StatusSignal x = imu.getQuatX(); + StatusSignal y = imu.getQuatY(); + StatusSignal z = imu.getQuatZ(); + return new Rotation3d( + new Quaternion( + w.refresh().getValue(), + x.refresh().getValue(), + y.refresh().getValue(), + z.refresh().getValue())); + } - /** - * Fetch the {@link Rotation3d} from the IMU. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - @Override - public Rotation3d getRotation3d() - { - return getRawRotation3d().minus(offset); - } + /** + * Fetch the {@link Rotation3d} from the IMU. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + @Override + public Rotation3d getRotation3d() { + return getRawRotation3d().minus(offset); + } - /** - * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns - * empty. - * - * @return {@link Translation3d} of the acceleration as an {@link Optional}. - */ - @Override - public Optional getAccel() - { - // TODO: Switch to suppliers. - StatusSignal xAcc = imu.getAccelerationX(); - StatusSignal yAcc = imu.getAccelerationX(); - StatusSignal zAcc = imu.getAccelerationX(); + /** + * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration + * isn't supported returns empty. + * + * @return {@link Translation3d} of the acceleration as an {@link Optional}. + */ + @Override + public Optional getAccel() { + // TODO: Switch to suppliers. + StatusSignal xAcc = imu.getAccelerationX(); + StatusSignal yAcc = imu.getAccelerationX(); + StatusSignal zAcc = imu.getAccelerationX(); - return Optional.of(new Translation3d(xAcc.refresh().getValue(), yAcc.refresh().getValue(), zAcc.refresh().getValue()).times(9.81 / 16384.0)); - } + return Optional.of( + new Translation3d( + xAcc.refresh().getValue(), yAcc.refresh().getValue(), zAcc.refresh().getValue()) + .times(9.81 / 16384.0)); + } - /** - * Get the instantiated IMU object. - * - * @return IMU object. - */ - @Override - public Object getIMU() - { - return imu; - } + /** + * Get the instantiated IMU object. + * + * @return IMU object. + */ + @Override + public Object getIMU() { + return imu; + } } diff --git a/src/main/java/swervelib/imu/PigeonSwerve.java b/src/main/java/swervelib/imu/PigeonSwerve.java index 44fafcc7..1132a888 100644 --- a/src/main/java/swervelib/imu/PigeonSwerve.java +++ b/src/main/java/swervelib/imu/PigeonSwerve.java @@ -7,107 +7,88 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.Optional; -/** - * SwerveIMU interface for the Pigeon. - */ -public class PigeonSwerve extends SwerveIMU -{ +/** SwerveIMU interface for the Pigeon. */ +public class PigeonSwerve extends SwerveIMU { - /** - * Pigeon v1 IMU device. - */ - WPI_PigeonIMU imu; - /** - * Offset for the Pigeon. - */ - private Rotation3d offset = new Rotation3d(); + /** Pigeon v1 IMU device. */ + WPI_PigeonIMU imu; + /** Offset for the Pigeon. */ + private Rotation3d offset = new Rotation3d(); - /** - * Generate the SwerveIMU for pigeon. - * - * @param canid CAN ID for the pigeon, does not support CANBus. - */ - public PigeonSwerve(int canid) - { - imu = new WPI_PigeonIMU(canid); - offset = new Rotation3d(); - SmartDashboard.putData(imu); - } + /** + * Generate the SwerveIMU for pigeon. + * + * @param canid CAN ID for the pigeon, does not support CANBus. + */ + public PigeonSwerve(int canid) { + imu = new WPI_PigeonIMU(canid); + offset = new Rotation3d(); + SmartDashboard.putData(imu); + } - /** - * Reset IMU to factory default. - */ - @Override - public void factoryDefault() - { - imu.configFactoryDefault(); - } + /** Reset IMU to factory default. */ + @Override + public void factoryDefault() { + imu.configFactoryDefault(); + } - /** - * Clear sticky faults on IMU. - */ - @Override - public void clearStickyFaults() - { - imu.clearStickyFaults(); - } + /** Clear sticky faults on IMU. */ + @Override + public void clearStickyFaults() { + imu.clearStickyFaults(); + } - /** - * Set the gyro offset. - * - * @param offset gyro offset as a {@link Rotation3d}. - */ - public void setOffset(Rotation3d offset) - { - this.offset = offset; - } + /** + * Set the gyro offset. + * + * @param offset gyro offset as a {@link Rotation3d}. + */ + public void setOffset(Rotation3d offset) { + this.offset = offset; + } - /** - * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - @Override - public Rotation3d getRawRotation3d() - { - double[] wxyz = new double[4]; - imu.get6dQuaternion(wxyz); - return new Rotation3d(new Quaternion(wxyz[0], wxyz[1], wxyz[2], wxyz[3])); - } + /** + * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + @Override + public Rotation3d getRawRotation3d() { + double[] wxyz = new double[4]; + imu.get6dQuaternion(wxyz); + return new Rotation3d(new Quaternion(wxyz[0], wxyz[1], wxyz[2], wxyz[3])); + } - /** - * Fetch the {@link Rotation3d} from the IMU. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - @Override - public Rotation3d getRotation3d() - { - return getRawRotation3d().minus(offset); - } + /** + * Fetch the {@link Rotation3d} from the IMU. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + @Override + public Rotation3d getRotation3d() { + return getRawRotation3d().minus(offset); + } - /** - * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns - * empty. - * - * @return {@link Translation3d} of the acceleration as an {@link Optional}. - */ - @Override - public Optional getAccel() - { - short[] initial = new short[3]; - imu.getBiasedAccelerometer(initial); - return Optional.of(new Translation3d(initial[0], initial[1], initial[2]).times(9.81 / 16384.0)); - } + /** + * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration + * isn't supported returns empty. + * + * @return {@link Translation3d} of the acceleration as an {@link Optional}. + */ + @Override + public Optional getAccel() { + short[] initial = new short[3]; + imu.getBiasedAccelerometer(initial); + return Optional.of(new Translation3d(initial[0], initial[1], initial[2]).times(9.81 / 16384.0)); + } - /** - * Get the instantiated IMU object. - * - * @return IMU object. - */ - @Override - public Object getIMU() - { - return imu; - } + /** + * Get the instantiated IMU object. + * + * @return IMU object. + */ + @Override + public Object getIMU() { + return imu; + } } diff --git a/src/main/java/swervelib/imu/SwerveIMU.java b/src/main/java/swervelib/imu/SwerveIMU.java index 11b39daa..f5a247f5 100644 --- a/src/main/java/swervelib/imu/SwerveIMU.java +++ b/src/main/java/swervelib/imu/SwerveIMU.java @@ -4,55 +4,48 @@ import edu.wpi.first.math.geometry.Translation3d; import java.util.Optional; -/** - * Swerve IMU abstraction to define a standard interface with a swerve drive. - */ -public abstract class SwerveIMU -{ - - /** - * Reset IMU to factory default. - */ - public abstract void factoryDefault(); - - /** - * Clear sticky faults on IMU. - */ - public abstract void clearStickyFaults(); - - /** - * Set the gyro offset. - * - * @param offset gyro offset as a {@link Rotation3d}. - */ - public abstract void setOffset(Rotation3d offset); - - /** - * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - public abstract Rotation3d getRawRotation3d(); - - /** - * Fetch the {@link Rotation3d} from the IMU. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - public abstract Rotation3d getRotation3d(); - - /** - * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns - * empty. - * - * @return {@link Translation3d} of the acceleration as an {@link Optional}. - */ - public abstract Optional getAccel(); - - /** - * Get the instantiated IMU object. - * - * @return IMU object. - */ - public abstract Object getIMU(); +/** Swerve IMU abstraction to define a standard interface with a swerve drive. */ +public abstract class SwerveIMU { + + /** Reset IMU to factory default. */ + public abstract void factoryDefault(); + + /** Clear sticky faults on IMU. */ + public abstract void clearStickyFaults(); + + /** + * Set the gyro offset. + * + * @param offset gyro offset as a {@link Rotation3d}. + */ + public abstract void setOffset(Rotation3d offset); + + /** + * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + public abstract Rotation3d getRawRotation3d(); + + /** + * Fetch the {@link Rotation3d} from the IMU. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + public abstract Rotation3d getRotation3d(); + + /** + * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration + * isn't supported returns empty. + * + * @return {@link Translation3d} of the acceleration as an {@link Optional}. + */ + public abstract Optional getAccel(); + + /** + * Get the instantiated IMU object. + * + * @return IMU object. + */ + public abstract Object getIMU(); } diff --git a/src/main/java/swervelib/imu/package-info.java b/src/main/java/swervelib/imu/package-info.java index 2b02abc0..f32f8463 100644 --- a/src/main/java/swervelib/imu/package-info.java +++ b/src/main/java/swervelib/imu/package-info.java @@ -1,4 +1,2 @@ -/** - * IMUs used for controlling the robot heading. All implement {@link swervelib.imu.SwerveIMU}. - */ +/** IMUs used for controlling the robot heading. All implement {@link swervelib.imu.SwerveIMU}. */ package swervelib.imu; diff --git a/src/main/java/swervelib/math/Matter.java b/src/main/java/swervelib/math/Matter.java index 94329b32..10d7aac6 100644 --- a/src/main/java/swervelib/math/Matter.java +++ b/src/main/java/swervelib/math/Matter.java @@ -2,40 +2,31 @@ import edu.wpi.first.math.geometry.Translation3d; -/** - * Object with significant mass that needs to be taken into account. - */ -public class Matter -{ +/** Object with significant mass that needs to be taken into account. */ +public class Matter { - /** - * Position in meters from robot center in 3d space. - */ - public Translation3d position; - /** - * Mass in kg of object. - */ - public double mass; + /** Position in meters from robot center in 3d space. */ + public Translation3d position; + /** Mass in kg of object. */ + public double mass; - /** - * Construct an object representing some significant matter on the robot. - * - * @param position Position of the matter in meters. - * @param mass Mass in kg. - */ - public Matter(Translation3d position, double mass) - { - this.mass = mass; - this.position = position; - } + /** + * Construct an object representing some significant matter on the robot. + * + * @param position Position of the matter in meters. + * @param mass Mass in kg. + */ + public Matter(Translation3d position, double mass) { + this.mass = mass; + this.position = position; + } - /** - * Get the center mass of the object. - * - * @return center mass = position * mass - */ - public Translation3d massMoment() - { - return position.times(mass); - } + /** + * Get the center mass of the object. + * + * @return center mass = position * mass + */ + public Translation3d massMoment() { + return position.times(mass); + } } diff --git a/src/main/java/swervelib/math/SwerveMath.java b/src/main/java/swervelib/math/SwerveMath.java index 0b4fdd5e..591d8f18 100644 --- a/src/main/java/swervelib/math/SwerveMath.java +++ b/src/main/java/swervelib/math/SwerveMath.java @@ -13,376 +13,345 @@ import swervelib.SwerveModule; import swervelib.parser.SwerveDriveConfiguration; import swervelib.parser.SwerveModuleConfiguration; -import swervelib.telemetry.SwerveDriveTelemetry; -import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; -/** - * Mathematical functions which pertain to swerve drive. - */ -public class SwerveMath -{ +/** Mathematical functions which pertain to swerve drive. */ +public class SwerveMath { - /** - * Calculate the meters per rotation for the integrated encoder. Calculation: (PI * WHEEL DIAMETER IN METERS) / (GEAR - * RATIO * ENCODER RESOLUTION) - * - * @param wheelDiameter Wheel diameter in meters. - * @param driveGearRatio The gear ratio of the drive motor. - * @param pulsePerRotation The number of encoder pulses per rotation. 1 if using an integrated encoder. - * @return Meters per rotation for the drive motor. - */ - public static double calculateMetersPerRotation( - double wheelDiameter, double driveGearRatio, double pulsePerRotation) - { - return (Math.PI * wheelDiameter) / (driveGearRatio * pulsePerRotation); - } + /** + * Calculate the meters per rotation for the integrated encoder. Calculation: (PI * WHEEL DIAMETER + * IN METERS) / (GEAR RATIO * ENCODER RESOLUTION) + * + * @param wheelDiameter Wheel diameter in meters. + * @param driveGearRatio The gear ratio of the drive motor. + * @param pulsePerRotation The number of encoder pulses per rotation. 1 if using an integrated + * encoder. + * @return Meters per rotation for the drive motor. + */ + public static double calculateMetersPerRotation( + double wheelDiameter, double driveGearRatio, double pulsePerRotation) { + return (Math.PI * wheelDiameter) / (driveGearRatio * pulsePerRotation); + } - /** - * Normalize an angle to be within 0 to 360. - * - * @param angle Angle in degrees. - * @return Normalized angle in degrees. - */ - public static double normalizeAngle(double angle) - { - Rotation2d angleRotation = Rotation2d.fromDegrees(angle); - return new Rotation2d(angleRotation.getCos(), angleRotation.getSin()).getDegrees(); - } + /** + * Normalize an angle to be within 0 to 360. + * + * @param angle Angle in degrees. + * @return Normalized angle in degrees. + */ + public static double normalizeAngle(double angle) { + Rotation2d angleRotation = Rotation2d.fromDegrees(angle); + return new Rotation2d(angleRotation.getCos(), angleRotation.getSin()).getDegrees(); + } - /** - * Algebraically apply a deadband using a piece wise function. - * - * @param value value to apply deadband to. - * @param scaled Use algebra to determine deadband by starting the value at 0 past deadband. - * @param deadband The deadbnad to apply. - * @return Value with deadband applied. - */ - public static double applyDeadband(double value, boolean scaled, double deadband) - { - value = Math.abs(value) > deadband ? value : 0; - return scaled - ? ((1 / (1 - deadband)) * (Math.abs(value) - deadband)) * Math.signum(value) - : value; - } + /** + * Algebraically apply a deadband using a piece wise function. + * + * @param value value to apply deadband to. + * @param scaled Use algebra to determine deadband by starting the value at 0 past deadband. + * @param deadband The deadbnad to apply. + * @return Value with deadband applied. + */ + public static double applyDeadband(double value, boolean scaled, double deadband) { + value = Math.abs(value) > deadband ? value : 0; + return scaled + ? ((1 / (1 - deadband)) * (Math.abs(value) - deadband)) * Math.signum(value) + : value; + } - /** - * Create the drive feedforward for swerve modules. - * - * @param optimalVoltage Optimal voltage to calculate kV (voltage/max Velocity) - * @param maxSpeed Maximum velocity in meters per second to use for the feed forward, should be - * as close to physical max as possible. - * @param wheelGripCoefficientOfFriction Wheel grip coefficient of friction for kA (voltage/(cof*9.81)) - * @return Drive feedforward for drive motor on a swerve module. - */ - public static SimpleMotorFeedforward createDriveFeedforward(double optimalVoltage, double maxSpeed, - double wheelGripCoefficientOfFriction) - { - double kv = optimalVoltage / maxSpeed; - /// ^ Volt-seconds per meter (max voltage divided by max speed) - double ka = - optimalVoltage - / calculateMaxAcceleration(wheelGripCoefficientOfFriction); - /// ^ Volt-seconds^2 per meter (max voltage divided by max accel) - return new SimpleMotorFeedforward(0, kv, ka); - } + /** + * Create the drive feedforward for swerve modules. + * + * @param optimalVoltage Optimal voltage to calculate kV (voltage/max Velocity) + * @param maxSpeed Maximum velocity in meters per second to use for the feed forward, should be as + * close to physical max as possible. + * @param wheelGripCoefficientOfFriction Wheel grip coefficient of friction for kA + * (voltage/(cof*9.81)) + * @return Drive feedforward for drive motor on a swerve module. + */ + public static SimpleMotorFeedforward createDriveFeedforward( + double optimalVoltage, double maxSpeed, double wheelGripCoefficientOfFriction) { + double kv = optimalVoltage / maxSpeed; + /// ^ Volt-seconds per meter (max voltage divided by max speed) + double ka = optimalVoltage / calculateMaxAcceleration(wheelGripCoefficientOfFriction); + /// ^ Volt-seconds^2 per meter (max voltage divided by max accel) + return new SimpleMotorFeedforward(0, kv, ka); + } - /** - * Calculate the degrees per steering rotation for the integrated encoder. Encoder conversion values. Drive converts - * motor rotations to linear wheel distance and steering converts motor rotations to module azimuth. - * - * @param angleGearRatio The gear ratio of the steering motor. - * @param pulsePerRotation The number of pulses in a complete rotation for the encoder, 1 if integrated. - * @return Degrees per steering rotation for the angle motor. - */ - public static double calculateDegreesPerSteeringRotation( - double angleGearRatio, double pulsePerRotation) - { - return 360 / (angleGearRatio * pulsePerRotation); - } + /** + * Calculate the degrees per steering rotation for the integrated encoder. Encoder conversion + * values. Drive converts motor rotations to linear wheel distance and steering converts motor + * rotations to module azimuth. + * + * @param angleGearRatio The gear ratio of the steering motor. + * @param pulsePerRotation The number of pulses in a complete rotation for the encoder, 1 if + * integrated. + * @return Degrees per steering rotation for the angle motor. + */ + public static double calculateDegreesPerSteeringRotation( + double angleGearRatio, double pulsePerRotation) { + return 360 / (angleGearRatio * pulsePerRotation); + } - /** - * Calculate the maximum angular velocity. - * - * @param maxSpeed Max speed of the robot in meters per second. - * @param furthestModuleX X of the furthest module in meters. - * @param furthestModuleY Y of the furthest module in meters. - * @return Maximum angular velocity in rad/s. - */ - public static double calculateMaxAngularVelocity( - double maxSpeed, double furthestModuleX, double furthestModuleY) - { - return maxSpeed / new Rotation2d(furthestModuleX, furthestModuleY).getRadians(); - } + /** + * Calculate the maximum angular velocity. + * + * @param maxSpeed Max speed of the robot in meters per second. + * @param furthestModuleX X of the furthest module in meters. + * @param furthestModuleY Y of the furthest module in meters. + * @return Maximum angular velocity in rad/s. + */ + public static double calculateMaxAngularVelocity( + double maxSpeed, double furthestModuleX, double furthestModuleY) { + return maxSpeed / new Rotation2d(furthestModuleX, furthestModuleY).getRadians(); + } - /** - * Calculate the practical maximum acceleration of the robot using the wheel coefficient of friction. - * - * @param cof Coefficient of Friction of the wheel grip tape. - * @return Practical maximum acceleration in m/s/s. - */ - public static double calculateMaxAcceleration(double cof) - { - return cof * 9.81; - } + /** + * Calculate the practical maximum acceleration of the robot using the wheel coefficient of + * friction. + * + * @param cof Coefficient of Friction of the wheel grip tape. + * @return Practical maximum acceleration in m/s/s. + */ + public static double calculateMaxAcceleration(double cof) { + return cof * 9.81; + } - /** - * Calculate the maximum theoretical acceleration without friction. - * - * @param stallTorqueNm Stall torque of driving motor in nM. - * @param gearRatio Gear ratio for driving motor number of motor rotations until one wheel rotation. - * @param moduleCount Number of swerve modules. - * @param wheelDiameter Wheel diameter in meters. - * @param robotMass Mass of the robot in kg. - * @return Theoretical maximum acceleration in m/s/s. - */ - public static double calculateMaxAcceleration( - double stallTorqueNm, - double gearRatio, - double moduleCount, - double wheelDiameter, - double robotMass) - { - return (stallTorqueNm * gearRatio * moduleCount) / ((wheelDiameter / 2) * robotMass); - } + /** + * Calculate the maximum theoretical acceleration without friction. + * + * @param stallTorqueNm Stall torque of driving motor in nM. + * @param gearRatio Gear ratio for driving motor number of motor rotations until one wheel + * rotation. + * @param moduleCount Number of swerve modules. + * @param wheelDiameter Wheel diameter in meters. + * @param robotMass Mass of the robot in kg. + * @return Theoretical maximum acceleration in m/s/s. + */ + public static double calculateMaxAcceleration( + double stallTorqueNm, + double gearRatio, + double moduleCount, + double wheelDiameter, + double robotMass) { + return (stallTorqueNm * gearRatio * moduleCount) / ((wheelDiameter / 2) * robotMass); + } - /** - * Calculates the maximum acceleration allowed in a direction without tipping the robot. Reads arm position from - * NetworkTables and is passed the direction in question. - * - * @param angle The direction in which to calculate max acceleration, as a Rotation2d. Note that this is - * robot-relative. - * @param matter Matter that the robot is composed of in kg. (Includes chassis) - * @param robotMass The weight of the robot in kg. (Including manipulators, etc). - * @param config The swerve drive configuration. - * @return Maximum acceleration allowed in the robot direction. - */ - private static double calcMaxAccel( - Rotation2d angle, - List matter, - double robotMass, - SwerveDriveConfiguration config) - { - // Calculate the vertical mass moment using the floor as the datum. This will be used later to - // calculate max acceleration - Translation3d centerMass = new Translation3d(); - for (Matter object : matter) - { - centerMass = centerMass.plus(object.massMoment()); - } - Translation3d robotCG = centerMass.div(robotMass); - Translation2d horizontalCG = robotCG.toTranslation2d(); + /** + * Calculates the maximum acceleration allowed in a direction without tipping the robot. Reads arm + * position from NetworkTables and is passed the direction in question. + * + * @param angle The direction in which to calculate max acceleration, as a Rotation2d. Note that + * this is robot-relative. + * @param matter Matter that the robot is composed of in kg. (Includes chassis) + * @param robotMass The weight of the robot in kg. (Including manipulators, etc). + * @param config The swerve drive configuration. + * @return Maximum acceleration allowed in the robot direction. + */ + private static double calcMaxAccel( + Rotation2d angle, List matter, double robotMass, SwerveDriveConfiguration config) { + // Calculate the vertical mass moment using the floor as the datum. This will be used later to + // calculate max acceleration + Translation3d centerMass = new Translation3d(); + for (Matter object : matter) { + centerMass = centerMass.plus(object.massMoment()); + } + Translation3d robotCG = centerMass.div(robotMass); + Translation2d horizontalCG = robotCG.toTranslation2d(); - Translation2d projectedHorizontalCg = - new Translation2d( - (angle.getSin() * angle.getCos() * horizontalCG.getY()) - + (Math.pow(angle.getCos(), 2) * horizontalCG.getX()), - (angle.getSin() * angle.getCos() * horizontalCG.getX()) - + (Math.pow(angle.getSin(), 2) * horizontalCG.getY())); + Translation2d projectedHorizontalCg = + new Translation2d( + (angle.getSin() * angle.getCos() * horizontalCG.getY()) + + (Math.pow(angle.getCos(), 2) * horizontalCG.getX()), + (angle.getSin() * angle.getCos() * horizontalCG.getX()) + + (Math.pow(angle.getSin(), 2) * horizontalCG.getY())); - // Projects the edge of the wheelbase onto the direction line. Assumes the wheelbase is - // rectangular. - // Because a line is being projected, rather than a point, one of the coordinates of the - // projected point is - // already known. - Translation2d projectedWheelbaseEdge; - double angDeg = angle.getDegrees(); - if (angDeg <= 45 && angDeg >= -45) - { - SwerveModuleConfiguration conf = getSwerveModule(config.modules, true, true); - projectedWheelbaseEdge = - new Translation2d( - conf.moduleLocation.getX(), conf.moduleLocation.getX() * angle.getTan()); - } else if (135 >= angDeg && angDeg > 45) - { - SwerveModuleConfiguration conf = getSwerveModule(config.modules, true, true); - projectedWheelbaseEdge = - new Translation2d( - conf.moduleLocation.getY() / angle.getTan(), conf.moduleLocation.getY()); - } else if (-135 <= angDeg && angDeg < -45) - { - SwerveModuleConfiguration conf = getSwerveModule(config.modules, true, false); - projectedWheelbaseEdge = - new Translation2d( - conf.moduleLocation.getY() / angle.getTan(), conf.moduleLocation.getY()); - } else - { - SwerveModuleConfiguration conf = getSwerveModule(config.modules, false, true); - projectedWheelbaseEdge = - new Translation2d( - conf.moduleLocation.getX(), conf.moduleLocation.getX() * angle.getTan()); - } + // Projects the edge of the wheelbase onto the direction line. Assumes the wheelbase is + // rectangular. + // Because a line is being projected, rather than a point, one of the coordinates of the + // projected point is + // already known. + Translation2d projectedWheelbaseEdge; + double angDeg = angle.getDegrees(); + if (angDeg <= 45 && angDeg >= -45) { + SwerveModuleConfiguration conf = getSwerveModule(config.modules, true, true); + projectedWheelbaseEdge = + new Translation2d( + conf.moduleLocation.getX(), conf.moduleLocation.getX() * angle.getTan()); + } else if (135 >= angDeg && angDeg > 45) { + SwerveModuleConfiguration conf = getSwerveModule(config.modules, true, true); + projectedWheelbaseEdge = + new Translation2d( + conf.moduleLocation.getY() / angle.getTan(), conf.moduleLocation.getY()); + } else if (-135 <= angDeg && angDeg < -45) { + SwerveModuleConfiguration conf = getSwerveModule(config.modules, true, false); + projectedWheelbaseEdge = + new Translation2d( + conf.moduleLocation.getY() / angle.getTan(), conf.moduleLocation.getY()); + } else { + SwerveModuleConfiguration conf = getSwerveModule(config.modules, false, true); + projectedWheelbaseEdge = + new Translation2d( + conf.moduleLocation.getX(), conf.moduleLocation.getX() * angle.getTan()); + } - double horizontalDistance = projectedHorizontalCg.plus(projectedWheelbaseEdge).getNorm(); - return 9.81 * horizontalDistance / robotCG.getZ(); - } + double horizontalDistance = projectedHorizontalCg.plus(projectedWheelbaseEdge).getNorm(); + return 9.81 * horizontalDistance / robotCG.getZ(); + } - /** - * Logical inverse of the Pose exponential from 254. Taken from team 3181. - * - * @param transform Pose to perform the log on. - * @return {@link Twist2d} of the transformed pose. - */ - public static Twist2d PoseLog(final Pose2d transform) - { + /** + * Logical inverse of the Pose exponential from 254. Taken from team 3181. + * + * @param transform Pose to perform the log on. + * @return {@link Twist2d} of the transformed pose. + */ + public static Twist2d PoseLog(final Pose2d transform) { - final double kEps = 1E-9; - final double dtheta = transform.getRotation().getRadians(); - final double half_dtheta = 0.5 * dtheta; - final double cos_minus_one = transform.getRotation().getCos() - 1.0; - double halftheta_by_tan_of_halfdtheta; - if (Math.abs(cos_minus_one) < kEps) - { - halftheta_by_tan_of_halfdtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta; - } else - { - halftheta_by_tan_of_halfdtheta = -(half_dtheta * transform.getRotation().getSin()) / cos_minus_one; - } - final Translation2d translation_part = transform.getTranslation() - .rotateBy(new Rotation2d(halftheta_by_tan_of_halfdtheta, - -half_dtheta)); - return new Twist2d(translation_part.getX(), translation_part.getY(), dtheta); - } + final double kEps = 1E-9; + final double dtheta = transform.getRotation().getRadians(); + final double half_dtheta = 0.5 * dtheta; + final double cos_minus_one = transform.getRotation().getCos() - 1.0; + double halftheta_by_tan_of_halfdtheta; + if (Math.abs(cos_minus_one) < kEps) { + halftheta_by_tan_of_halfdtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta; + } else { + halftheta_by_tan_of_halfdtheta = + -(half_dtheta * transform.getRotation().getSin()) / cos_minus_one; + } + final Translation2d translation_part = + transform + .getTranslation() + .rotateBy(new Rotation2d(halftheta_by_tan_of_halfdtheta, -half_dtheta)); + return new Twist2d(translation_part.getX(), translation_part.getY(), dtheta); + } - /** - * Limits a commanded velocity to prevent exceeding the maximum acceleration given by {@link SwerveMath#calcMaxAccel}. - * Note that this takes and returns field-relative velocities. - * - * @param commandedVelocity The desired velocity - * @param fieldVelocity The velocity of the robot within a field relative state. - * @param robotPose The current pose of the robot. - * @param loopTime The time it takes to update the velocity in seconds. Note: this should include the - * 100ms that it takes for a SparkMax velocity to update. - * @param matter Matter that the robot is composed of with position in meters and mass in kg. - * @param robotMass The weight of the robot in kg. (Including manipulators, etc). - * @param config The swerve drive configuration. - * @return The limited velocity. This is either the commanded velocity, if attainable, or the closest attainable - * velocity. - */ - public static Translation2d limitVelocity( - Translation2d commandedVelocity, - ChassisSpeeds fieldVelocity, - Pose2d robotPose, - double loopTime, - double robotMass, - List matter, - SwerveDriveConfiguration config) - { - // Get the robot's current field-relative velocity - Translation2d currentVelocity = SwerveController.getTranslation2d(fieldVelocity); + /** + * Limits a commanded velocity to prevent exceeding the maximum acceleration given by {@link + * SwerveMath#calcMaxAccel}. Note that this takes and returns field-relative velocities. + * + * @param commandedVelocity The desired velocity + * @param fieldVelocity The velocity of the robot within a field relative state. + * @param robotPose The current pose of the robot. + * @param loopTime The time it takes to update the velocity in seconds. Note: this should + * include the 100ms that it takes for a SparkMax velocity to update. + * @param matter Matter that the robot is composed of with position in meters and mass in kg. + * @param robotMass The weight of the robot in kg. (Including manipulators, etc). + * @param config The swerve drive configuration. + * @return The limited velocity. This is either the commanded velocity, if attainable, or the + * closest attainable velocity. + */ + public static Translation2d limitVelocity( + Translation2d commandedVelocity, + ChassisSpeeds fieldVelocity, + Pose2d robotPose, + double loopTime, + double robotMass, + List matter, + SwerveDriveConfiguration config) { + // Get the robot's current field-relative velocity + Translation2d currentVelocity = SwerveController.getTranslation2d(fieldVelocity); - // Calculate the commanded change in velocity by subtracting current velocity - // from commanded velocity - Translation2d deltaV = commandedVelocity.minus(currentVelocity); + // Calculate the commanded change in velocity by subtracting current velocity + // from commanded velocity + Translation2d deltaV = commandedVelocity.minus(currentVelocity); - // Creates an acceleration vector with the direction of delta V and a magnitude - // of the maximum allowed acceleration in that direction - Translation2d maxAccel = - new Translation2d( - calcMaxAccel( - deltaV - // Rotates the velocity vector to convert from field-relative to robot-relative - .rotateBy(robotPose.getRotation().unaryMinus()) - .getAngle(), - matter, - robotMass, - config), - deltaV.getAngle()); + // Creates an acceleration vector with the direction of delta V and a magnitude + // of the maximum allowed acceleration in that direction + Translation2d maxAccel = + new Translation2d( + calcMaxAccel( + deltaV + // Rotates the velocity vector to convert from field-relative to robot-relative + .rotateBy(robotPose.getRotation().unaryMinus()) + .getAngle(), + matter, + robotMass, + config), + deltaV.getAngle()); - // Calculate the maximum achievable velocity by the next loop cycle. - // delta V = Vf - Vi = at - Translation2d maxAchievableDeltaVelocity = maxAccel.times(loopTime); + // Calculate the maximum achievable velocity by the next loop cycle. + // delta V = Vf - Vi = at + Translation2d maxAchievableDeltaVelocity = maxAccel.times(loopTime); - if (deltaV.getNorm() > maxAchievableDeltaVelocity.getNorm()) - { - return maxAchievableDeltaVelocity.plus(currentVelocity); - } else - { - // If the commanded velocity is attainable, use that. - return commandedVelocity; - } - } + if (deltaV.getNorm() > maxAchievableDeltaVelocity.getNorm()) { + return maxAchievableDeltaVelocity.plus(currentVelocity); + } else { + // If the commanded velocity is attainable, use that. + return commandedVelocity; + } + } - /** - * Get the fruthest module from center based on the module locations. - * - * @param modules Swerve module list. - * @param front True = furthest front, False = furthest back. - * @param left True = furthest left, False = furthest right. - * @return Module location which is the furthest from center and abides by parameters. - */ - public static SwerveModuleConfiguration getSwerveModule( - SwerveModule[] modules, boolean front, boolean left) - { - Translation2d target = modules[0].configuration.moduleLocation, current, temp; - SwerveModuleConfiguration configuration = modules[0].configuration; - for (SwerveModule module : modules) - { - current = module.configuration.moduleLocation; - temp = - front - ? (target.getY() >= current.getY() ? current : target) - : (target.getY() <= current.getY() ? current : target); - target = - left - ? (target.getX() >= temp.getX() ? temp : target) - : (target.getX() <= temp.getX() ? temp : target); - configuration = current.equals(target) ? module.configuration : configuration; - } - return configuration; - } + /** + * Get the fruthest module from center based on the module locations. + * + * @param modules Swerve module list. + * @param front True = furthest front, False = furthest back. + * @param left True = furthest left, False = furthest right. + * @return Module location which is the furthest from center and abides by parameters. + */ + public static SwerveModuleConfiguration getSwerveModule( + SwerveModule[] modules, boolean front, boolean left) { + Translation2d target = modules[0].configuration.moduleLocation, current, temp; + SwerveModuleConfiguration configuration = modules[0].configuration; + for (SwerveModule module : modules) { + current = module.configuration.moduleLocation; + temp = + front + ? (target.getY() >= current.getY() ? current : target) + : (target.getY() <= current.getY() ? current : target); + target = + left + ? (target.getX() >= temp.getX() ? temp : target) + : (target.getX() <= temp.getX() ? temp : target); + configuration = current.equals(target) ? module.configuration : configuration; + } + return configuration; + } - /** - * Put an angle within the 360 deg scope of a reference. For example, given a scope reference of 756 degrees, assumes - * the full scope is (720-1080), and places an angle of 22 degrees into it, returning 742 deg. - * - * @param scopeReference Current Angle (deg) - * @param newAngle Target Angle (deg) - * @return Closest angle within scope (deg) - */ - public static double placeInAppropriate0To360Scope(double scopeReference, double newAngle) - { - double lowerBound; - double upperBound; - double lowerOffset = scopeReference % 360; - if (lowerOffset >= 0) - { - lowerBound = scopeReference - lowerOffset; - upperBound = scopeReference + (360 - lowerOffset); - } else - { - upperBound = scopeReference - lowerOffset; - lowerBound = scopeReference - (360 + lowerOffset); - } - while (newAngle < lowerBound) - { - newAngle += 360; - } - while (newAngle > upperBound) - { - newAngle -= 360; - } - if (newAngle - scopeReference > 180) - { - newAngle -= 360; - } else if (newAngle - scopeReference < -180) - { - newAngle += 360; - } - return newAngle; - } + /** + * Put an angle within the 360 deg scope of a reference. For example, given a scope reference of + * 756 degrees, assumes the full scope is (720-1080), and places an angle of 22 degrees into it, + * returning 742 deg. + * + * @param scopeReference Current Angle (deg) + * @param newAngle Target Angle (deg) + * @return Closest angle within scope (deg) + */ + public static double placeInAppropriate0To360Scope(double scopeReference, double newAngle) { + double lowerBound; + double upperBound; + double lowerOffset = scopeReference % 360; + if (lowerOffset >= 0) { + lowerBound = scopeReference - lowerOffset; + upperBound = scopeReference + (360 - lowerOffset); + } else { + upperBound = scopeReference - lowerOffset; + lowerBound = scopeReference - (360 + lowerOffset); + } + while (newAngle < lowerBound) { + newAngle += 360; + } + while (newAngle > upperBound) { + newAngle -= 360; + } + if (newAngle - scopeReference > 180) { + newAngle -= 360; + } else if (newAngle - scopeReference < -180) { + newAngle += 360; + } + return newAngle; + } - /** - * Perform anti-jitter within modules if the speed requested is too low. - * - * @param moduleState Current {@link SwerveModuleState} requested. - * @param lastModuleState Previous {@link SwerveModuleState} used. - * @param maxSpeed Maximum speed of the modules. - */ - public static void antiJitter(SwerveModuleState moduleState, SwerveModuleState lastModuleState, double maxSpeed) - { - if (Math.abs(moduleState.speedMetersPerSecond) <= (maxSpeed * 0.01)) - { - moduleState.angle = lastModuleState.angle; - } - } + /** + * Perform anti-jitter within modules if the speed requested is too low. + * + * @param moduleState Current {@link SwerveModuleState} requested. + * @param lastModuleState Previous {@link SwerveModuleState} used. + * @param maxSpeed Maximum speed of the modules. + */ + public static void antiJitter( + SwerveModuleState moduleState, SwerveModuleState lastModuleState, double maxSpeed) { + if (Math.abs(moduleState.speedMetersPerSecond) <= (maxSpeed * 0.01)) { + moduleState.angle = lastModuleState.angle; + } + } } diff --git a/src/main/java/swervelib/math/package-info.java b/src/main/java/swervelib/math/package-info.java index 0bfafd54..633b280c 100644 --- a/src/main/java/swervelib/math/package-info.java +++ b/src/main/java/swervelib/math/package-info.java @@ -1,6 +1,6 @@ /** - * Mathematics for swerve drives. Original second order kinematics was developed by Team 3181 - * here. + * Mathematics for swerve drives. Original second order kinematics was developed by Team 3181 here. * */ package swervelib.math; diff --git a/src/main/java/swervelib/motors/SparkFlexSwerve.java b/src/main/java/swervelib/motors/SparkFlexSwerve.java index a5aef968..ff5c59ea 100644 --- a/src/main/java/swervelib/motors/SparkFlexSwerve.java +++ b/src/main/java/swervelib/motors/SparkFlexSwerve.java @@ -1,12 +1,12 @@ package swervelib.motors; import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.CANSparkFlex; -import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkLowLevel.PeriodicFrame; +import com.revrobotics.CANSparkMax; import com.revrobotics.MotorFeedbackSensor; import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; @@ -18,430 +18,361 @@ import swervelib.parser.PIDFConfig; import swervelib.telemetry.SwerveDriveTelemetry; -/** - * An implementation of {@link CANSparkFlex} as a {@link SwerveMotor}. - */ -public class SparkFlexSwerve extends SwerveMotor -{ - - /** - * SparkMAX Instance. - */ - public CANSparkFlex motor; - /** - * Integrated encoder. - */ - public RelativeEncoder encoder; - /** - * Absolute encoder attached to the SparkMax (if exists) - */ - public SwerveAbsoluteEncoder absoluteEncoder; - /** - * Closed-loop PID controller. - */ - public SparkPIDController pid; - /** - * Factory default already occurred. - */ - private boolean factoryDefaultOccurred = false; - - /** - * Initialize the swerve motor. - * - * @param motor The SwerveMotor as a SparkFlex object. - * @param isDriveMotor Is the motor being initialized a drive motor? - */ - public SparkFlexSwerve(CANSparkFlex motor, boolean isDriveMotor) - { - this.motor = motor; - this.isDriveMotor = isDriveMotor; - factoryDefaults(); - clearStickyFaults(); - - encoder = motor.getEncoder(); - pid = motor.getPIDController(); - pid.setFeedbackDevice( - encoder); // Configure feedback of the PID controller as the integrated encoder. - - // Spin off configurations in a different thread. - // configureSparkMax(() -> motor.setCANTimeout(0)); // Commented out because it prevents feedback. - } - - /** - * Initialize the {@link SwerveMotor} as a {@link CANSparkMax} connected to a Brushless Motor. - * - * @param id CAN ID of the SparkMax. - * @param isDriveMotor Is the motor being initialized a drive motor? - */ - public SparkFlexSwerve(int id, boolean isDriveMotor) - { - this(new CANSparkFlex(id, MotorType.kBrushless), isDriveMotor); - } - - /** - * Run the configuration until it succeeds or times out. - * - * @param config Lambda supplier returning the error state. - */ - private void configureSparkFlex(Supplier config) - { - for (int i = 0; i < maximumRetries; i++) - { - if (config.get() == REVLibError.kOk) - { - return; - } - } - DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true); - } - - /** - * Set the voltage compensation for the swerve module motor. - * - * @param nominalVoltage Nominal voltage for operation to output to. - */ - @Override - public void setVoltageCompensation(double nominalVoltage) - { - configureSparkFlex(() -> motor.enableVoltageCompensation(nominalVoltage)); - } - - /** - * Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with - * voltage compensation. This is useful to protect the motor from current spikes. - * - * @param currentLimit Current limit in AMPS at free speed. - */ - @Override - public void setCurrentLimit(int currentLimit) - { - configureSparkFlex(() -> motor.setSmartCurrentLimit(currentLimit)); - } - - /** - * Set the maximum rate the open/closed loop output can change by. - * - * @param rampRate Time in seconds to go from 0 to full throttle. - */ - @Override - public void setLoopRampRate(double rampRate) - { - configureSparkFlex(() -> motor.setOpenLoopRampRate(rampRate)); - configureSparkFlex(() -> motor.setClosedLoopRampRate(rampRate)); - } - - /** - * Get the motor object from the module. - * - * @return Motor object. - */ - @Override - public Object getMotor() - { - return motor; - } - - /** - * Queries whether the absolute encoder is directly attached to the motor controller. - * - * @return connected absolute encoder state. - */ - @Override - public boolean isAttachedAbsoluteEncoder() - { - return absoluteEncoder != null; - } - - /** - * Configure the factory defaults. - */ - @Override - public void factoryDefaults() - { - if (!factoryDefaultOccurred) - { - configureSparkFlex(motor::restoreFactoryDefaults); - factoryDefaultOccurred = true; - } - } - - /** - * Clear the sticky faults on the motor controller. - */ - @Override - public void clearStickyFaults() - { - configureSparkFlex(motor::clearFaults); - } - - /** - * Set the absolute encoder to be a compatible absolute encoder. - * - * @param encoder The encoder to use. - * @return The {@link SwerveMotor} for easy instantiation. - */ - @Override - public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) - { - if (encoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) - { - DriverStation.reportWarning( - "IF possible configure the duty cycle encoder offset in the REV Hardware Client instead of using the" + - " absoluteEncoderOffset in the Swerve Module JSON!", - false); - absoluteEncoder = encoder; - configureSparkFlex(() -> pid.setFeedbackDevice((MotorFeedbackSensor) absoluteEncoder.getAbsoluteEncoder())); - } - return this; - } - - /** - * Configure the integrated encoder for the swerve module. Sets the conversion factors for position and velocity. - * - * @param positionConversionFactor The conversion factor to apply. - */ - @Override - public void configureIntegratedEncoder(double positionConversionFactor) - { - if (absoluteEncoder == null) - { - configureSparkFlex(() -> encoder.setPositionConversionFactor(positionConversionFactor)); - configureSparkFlex(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60)); - - // Taken from - // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67 - configureCANStatusFrames(10, 20, 20, 500, 500); - } else - { - configureSparkFlex(() -> { - if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) - { - return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor( - positionConversionFactor); - } else - { - return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor( - positionConversionFactor); - } - }); - configureSparkFlex(() -> { - if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) - { - return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor( - positionConversionFactor / 60); - } else - { - return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor( - positionConversionFactor / 60); - } - }); - } - } - - /** - * Configure the PIDF values for the closed loop controller. - * - * @param config Configuration class holding the PIDF values. - */ - @Override - public void configurePIDF(PIDFConfig config) - { -// int pidSlot = -// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal(); - int pidSlot = 0; - configureSparkFlex(() -> pid.setP(config.p, pidSlot)); - configureSparkFlex(() -> pid.setI(config.i, pidSlot)); - configureSparkFlex(() -> pid.setD(config.d, pidSlot)); - configureSparkFlex(() -> pid.setFF(config.f, pidSlot)); - configureSparkFlex(() -> pid.setIZone(config.iz, pidSlot)); - configureSparkFlex(() -> pid.setOutputRange(config.output.min, config.output.max, pidSlot)); - } - - /** - * Configure the PID wrapping for the position closed loop controller. - * - * @param minInput Minimum PID input. - * @param maxInput Maximum PID input. - */ - @Override - public void configurePIDWrapping(double minInput, double maxInput) - { - configureSparkFlex(() -> pid.setPositionPIDWrappingEnabled(true)); - configureSparkFlex(() -> pid.setPositionPIDWrappingMinInput(minInput)); - configureSparkFlex(() -> pid.setPositionPIDWrappingMaxInput(maxInput)); - } - - /** - * Set the CAN status frames. - * - * @param CANStatus0 Applied Output, Faults, Sticky Faults, Is Follower - * @param CANStatus1 Motor Velocity, Motor Temperature, Motor Voltage, Motor Current - * @param CANStatus2 Motor Position - * @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position - * @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position - */ - public void configureCANStatusFrames( - int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4) - { - configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0)); - configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1)); - configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2)); - configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3)); - configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4)); - // TODO: Configure Status Frame 5 and 6 if necessary - // https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces - } - - /** - * Set the idle mode. - * - * @param isBrakeMode Set the brake mode. - */ - @Override - public void setMotorBrake(boolean isBrakeMode) - { - configureSparkFlex(() -> motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast)); - } - - /** - * Set the motor to be inverted. - * - * @param inverted State of inversion. - */ - @Override - public void setInverted(boolean inverted) - { - motor.setInverted(inverted); - } - - /** - * Save the configurations from flash to EEPROM. - */ - @Override - public void burnFlash() - { - try - { - Thread.sleep(200); - } catch (Exception e) - { - } - configureSparkFlex(() -> motor.burnFlash()); - } - - /** - * Set the percentage output. - * - * @param percentOutput percent out for the motor controller. - */ - @Override - public void set(double percentOutput) - { - motor.set(percentOutput); - } - - /** - * Set the closed loop PID controller reference point. - * - * @param setpoint Setpoint in MPS or Angle in degrees. - * @param feedforward Feedforward in volt-meter-per-second or kV. - */ - @Override - public void setReference(double setpoint, double feedforward) - { - boolean possibleBurnOutIssue = true; -// int pidSlot = -// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal(); - int pidSlot = 0; - - if (isDriveMotor) - { - configureSparkFlex(() -> - pid.setReference( - setpoint, - ControlType.kVelocity, - pidSlot, - feedforward)); - } else - { - configureSparkFlex(() -> - pid.setReference( - setpoint, - ControlType.kPosition, - pidSlot, - feedforward)); - if(SwerveDriveTelemetry.isSimulation) - { - encoder.setPosition(setpoint); - } - } - } - - /** - * Set the closed loop PID controller reference point. - * - * @param setpoint Setpoint in meters per second or angle in degrees. - * @param feedforward Feedforward in volt-meter-per-second or kV. - * @param position Only used on the angle motor, the position of the motor in degrees. - */ - @Override - public void setReference(double setpoint, double feedforward, double position) - { - setReference(setpoint, feedforward); - } - - /** - * Get the velocity of the integrated encoder. - * - * @return velocity - */ - @Override - public double getVelocity() - { - return absoluteEncoder == null ? encoder.getVelocity() : absoluteEncoder.getVelocity(); - } - - /** - * Get the position of the integrated encoder. - * - * @return Position - */ - @Override - public double getPosition() - { - return absoluteEncoder == null ? encoder.getPosition() : absoluteEncoder.getAbsolutePosition(); - } - - /** - * Set the integrated encoder position. - * - * @param position Integrated encoder position. - */ - @Override - public void setPosition(double position) - { - if (absoluteEncoder == null) - { - configureSparkFlex(() -> encoder.setPosition(position)); - } - } - - /** - * REV Slots for PID configuration. - */ - enum SparkMAX_slotIdx - { - /** - * Slot 1, used for position PID's. - */ - Position, - /** - * Slot 2, used for velocity PID's. - */ - Velocity, - /** - * Slot 3, used arbitrarily. (Documentation show simulations). - */ - Simulation - } +/** An implementation of {@link CANSparkFlex} as a {@link SwerveMotor}. */ +public class SparkFlexSwerve extends SwerveMotor { + + /** SparkMAX Instance. */ + public CANSparkFlex motor; + /** Integrated encoder. */ + public RelativeEncoder encoder; + /** Absolute encoder attached to the SparkMax (if exists) */ + public SwerveAbsoluteEncoder absoluteEncoder; + /** Closed-loop PID controller. */ + public SparkPIDController pid; + /** Factory default already occurred. */ + private boolean factoryDefaultOccurred = false; + + /** + * Initialize the swerve motor. + * + * @param motor The SwerveMotor as a SparkFlex object. + * @param isDriveMotor Is the motor being initialized a drive motor? + */ + public SparkFlexSwerve(CANSparkFlex motor, boolean isDriveMotor) { + this.motor = motor; + this.isDriveMotor = isDriveMotor; + factoryDefaults(); + clearStickyFaults(); + + encoder = motor.getEncoder(); + pid = motor.getPIDController(); + pid.setFeedbackDevice( + encoder); // Configure feedback of the PID controller as the integrated encoder. + + // Spin off configurations in a different thread. + // configureSparkMax(() -> motor.setCANTimeout(0)); // Commented out because it prevents + // feedback. + } + + /** + * Initialize the {@link SwerveMotor} as a {@link CANSparkMax} connected to a Brushless Motor. + * + * @param id CAN ID of the SparkMax. + * @param isDriveMotor Is the motor being initialized a drive motor? + */ + public SparkFlexSwerve(int id, boolean isDriveMotor) { + this(new CANSparkFlex(id, MotorType.kBrushless), isDriveMotor); + } + + /** + * Run the configuration until it succeeds or times out. + * + * @param config Lambda supplier returning the error state. + */ + private void configureSparkFlex(Supplier config) { + for (int i = 0; i < maximumRetries; i++) { + if (config.get() == REVLibError.kOk) { + return; + } + } + DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true); + } + + /** + * Set the voltage compensation for the swerve module motor. + * + * @param nominalVoltage Nominal voltage for operation to output to. + */ + @Override + public void setVoltageCompensation(double nominalVoltage) { + configureSparkFlex(() -> motor.enableVoltageCompensation(nominalVoltage)); + } + + /** + * Set the current limit for the swerve drive motor, remember this may cause jumping if used in + * conjunction with voltage compensation. This is useful to protect the motor from current spikes. + * + * @param currentLimit Current limit in AMPS at free speed. + */ + @Override + public void setCurrentLimit(int currentLimit) { + configureSparkFlex(() -> motor.setSmartCurrentLimit(currentLimit)); + } + + /** + * Set the maximum rate the open/closed loop output can change by. + * + * @param rampRate Time in seconds to go from 0 to full throttle. + */ + @Override + public void setLoopRampRate(double rampRate) { + configureSparkFlex(() -> motor.setOpenLoopRampRate(rampRate)); + configureSparkFlex(() -> motor.setClosedLoopRampRate(rampRate)); + } + + /** + * Get the motor object from the module. + * + * @return Motor object. + */ + @Override + public Object getMotor() { + return motor; + } + + /** + * Queries whether the absolute encoder is directly attached to the motor controller. + * + * @return connected absolute encoder state. + */ + @Override + public boolean isAttachedAbsoluteEncoder() { + return absoluteEncoder != null; + } + + /** Configure the factory defaults. */ + @Override + public void factoryDefaults() { + if (!factoryDefaultOccurred) { + configureSparkFlex(motor::restoreFactoryDefaults); + factoryDefaultOccurred = true; + } + } + + /** Clear the sticky faults on the motor controller. */ + @Override + public void clearStickyFaults() { + configureSparkFlex(motor::clearFaults); + } + + /** + * Set the absolute encoder to be a compatible absolute encoder. + * + * @param encoder The encoder to use. + * @return The {@link SwerveMotor} for easy instantiation. + */ + @Override + public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) { + if (encoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) { + DriverStation.reportWarning( + "IF possible configure the duty cycle encoder offset in the REV Hardware Client instead of using the" + + " absoluteEncoderOffset in the Swerve Module JSON!", + false); + absoluteEncoder = encoder; + configureSparkFlex( + () -> pid.setFeedbackDevice((MotorFeedbackSensor) absoluteEncoder.getAbsoluteEncoder())); + } + return this; + } + + /** + * Configure the integrated encoder for the swerve module. Sets the conversion factors for + * position and velocity. + * + * @param positionConversionFactor The conversion factor to apply. + */ + @Override + public void configureIntegratedEncoder(double positionConversionFactor) { + if (absoluteEncoder == null) { + configureSparkFlex(() -> encoder.setPositionConversionFactor(positionConversionFactor)); + configureSparkFlex(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60)); + + // Taken from + // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67 + configureCANStatusFrames(10, 20, 20, 500, 500); + } else { + configureSparkFlex( + () -> { + if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) { + return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()) + .setPositionConversionFactor(positionConversionFactor); + } else { + return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()) + .setPositionConversionFactor(positionConversionFactor); + } + }); + configureSparkFlex( + () -> { + if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) { + return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()) + .setVelocityConversionFactor(positionConversionFactor / 60); + } else { + return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()) + .setVelocityConversionFactor(positionConversionFactor / 60); + } + }); + } + } + + /** + * Configure the PIDF values for the closed loop controller. + * + * @param config Configuration class holding the PIDF values. + */ + @Override + public void configurePIDF(PIDFConfig config) { + // int pidSlot = + // isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : + // SparkMAX_slotIdx.Position.ordinal(); + int pidSlot = 0; + configureSparkFlex(() -> pid.setP(config.p, pidSlot)); + configureSparkFlex(() -> pid.setI(config.i, pidSlot)); + configureSparkFlex(() -> pid.setD(config.d, pidSlot)); + configureSparkFlex(() -> pid.setFF(config.f, pidSlot)); + configureSparkFlex(() -> pid.setIZone(config.iz, pidSlot)); + configureSparkFlex(() -> pid.setOutputRange(config.output.min, config.output.max, pidSlot)); + } + + /** + * Configure the PID wrapping for the position closed loop controller. + * + * @param minInput Minimum PID input. + * @param maxInput Maximum PID input. + */ + @Override + public void configurePIDWrapping(double minInput, double maxInput) { + configureSparkFlex(() -> pid.setPositionPIDWrappingEnabled(true)); + configureSparkFlex(() -> pid.setPositionPIDWrappingMinInput(minInput)); + configureSparkFlex(() -> pid.setPositionPIDWrappingMaxInput(maxInput)); + } + + /** + * Set the CAN status frames. + * + * @param CANStatus0 Applied Output, Faults, Sticky Faults, Is Follower + * @param CANStatus1 Motor Velocity, Motor Temperature, Motor Voltage, Motor Current + * @param CANStatus2 Motor Position + * @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position + * @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position + */ + public void configureCANStatusFrames( + int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4) { + configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0)); + configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1)); + configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2)); + configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3)); + configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4)); + // TODO: Configure Status Frame 5 and 6 if necessary + // https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces + } + + /** + * Set the idle mode. + * + * @param isBrakeMode Set the brake mode. + */ + @Override + public void setMotorBrake(boolean isBrakeMode) { + configureSparkFlex(() -> motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast)); + } + + /** + * Set the motor to be inverted. + * + * @param inverted State of inversion. + */ + @Override + public void setInverted(boolean inverted) { + motor.setInverted(inverted); + } + + /** Save the configurations from flash to EEPROM. */ + @Override + public void burnFlash() { + try { + Thread.sleep(200); + } catch (Exception e) { + } + configureSparkFlex(() -> motor.burnFlash()); + } + + /** + * Set the percentage output. + * + * @param percentOutput percent out for the motor controller. + */ + @Override + public void set(double percentOutput) { + motor.set(percentOutput); + } + + /** + * Set the closed loop PID controller reference point. + * + * @param setpoint Setpoint in MPS or Angle in degrees. + * @param feedforward Feedforward in volt-meter-per-second or kV. + */ + @Override + public void setReference(double setpoint, double feedforward) { + boolean possibleBurnOutIssue = true; + // int pidSlot = + // isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : + // SparkMAX_slotIdx.Position.ordinal(); + int pidSlot = 0; + + if (isDriveMotor) { + configureSparkFlex( + () -> pid.setReference(setpoint, ControlType.kVelocity, pidSlot, feedforward)); + } else { + configureSparkFlex( + () -> pid.setReference(setpoint, ControlType.kPosition, pidSlot, feedforward)); + if (SwerveDriveTelemetry.isSimulation) { + encoder.setPosition(setpoint); + } + } + } + + /** + * Set the closed loop PID controller reference point. + * + * @param setpoint Setpoint in meters per second or angle in degrees. + * @param feedforward Feedforward in volt-meter-per-second or kV. + * @param position Only used on the angle motor, the position of the motor in degrees. + */ + @Override + public void setReference(double setpoint, double feedforward, double position) { + setReference(setpoint, feedforward); + } + + /** + * Get the velocity of the integrated encoder. + * + * @return velocity + */ + @Override + public double getVelocity() { + return absoluteEncoder == null ? encoder.getVelocity() : absoluteEncoder.getVelocity(); + } + + /** + * Get the position of the integrated encoder. + * + * @return Position + */ + @Override + public double getPosition() { + return absoluteEncoder == null ? encoder.getPosition() : absoluteEncoder.getAbsolutePosition(); + } + + /** + * Set the integrated encoder position. + * + * @param position Integrated encoder position. + */ + @Override + public void setPosition(double position) { + if (absoluteEncoder == null) { + configureSparkFlex(() -> encoder.setPosition(position)); + } + } + + /** REV Slots for PID configuration. */ + enum SparkMAX_slotIdx { + /** Slot 1, used for position PID's. */ + Position, + /** Slot 2, used for velocity PID's. */ + Velocity, + /** Slot 3, used arbitrarily. (Documentation show simulations). */ + Simulation + } } diff --git a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java index fc7d94ed..97d4e3b4 100644 --- a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -1,11 +1,11 @@ package swervelib.motors; import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkLowLevel.PeriodicFrame; +import com.revrobotics.CANSparkMax; import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxAlternateEncoder; @@ -16,403 +16,374 @@ import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; -/** - * Brushed motor control with SparkMax. - */ -public class SparkMaxBrushedMotorSwerve extends SwerveMotor -{ - - /** - * SparkMAX Instance. - */ - public CANSparkMax motor; - - /** - * Absolute encoder attached to the SparkMax (if exists) - */ - public AbsoluteEncoder absoluteEncoder; - /** - * Integrated encoder. - */ - public RelativeEncoder encoder; - /** - * Closed-loop PID controller. - */ - public SparkPIDController pid; - /** - * Factory default already occurred. - */ - private boolean factoryDefaultOccurred = false; - - /** - * Initialize the swerve motor. - * - * @param motor The SwerveMotor as a SparkMax object. - * @param isDriveMotor Is the motor being initialized a drive motor? - * @param encoderType {@link Type} of encoder to use for the {@link CANSparkMax} device. - * @param countsPerRevolution The number of encoder pulses for the {@link Type} encoder per revolution. - * @param useDataPortQuadEncoder Use the encoder attached to the data port of the spark max for a quadrature encoder. - */ - public SparkMaxBrushedMotorSwerve(CANSparkMax motor, boolean isDriveMotor, Type encoderType, int countsPerRevolution, - boolean useDataPortQuadEncoder) - { - // Drive motors **MUST** have an encoder attached. - if (isDriveMotor && encoderType == Type.kNoSensor) - { - DriverStation.reportError("Cannot use motor without encoder.", true); - throw new RuntimeException("Cannot use SparkMAX as a drive motor without an encoder attached."); - } - - // Hall encoders can be used as quadrature encoders. - if (encoderType == Type.kHallSensor) - { - encoderType = Type.kQuadrature; - } - - this.motor = motor; - this.isDriveMotor = isDriveMotor; - - factoryDefaults(); - clearStickyFaults(); - - // Get the onboard PID controller. - pid = motor.getPIDController(); - - // If there is a sensor attached to the data port or encoder port set the relative encoder. - if (isDriveMotor || (encoderType != Type.kNoSensor || useDataPortQuadEncoder)) - { - this.encoder = useDataPortQuadEncoder ? - motor.getAlternateEncoder(SparkMaxAlternateEncoder.Type.kQuadrature, countsPerRevolution) : - motor.getEncoder(encoderType, countsPerRevolution); - - // Configure feedback of the PID controller as the integrated encoder. - configureSparkMax(() -> pid.setFeedbackDevice(encoder)); - } - // Spin off configurations in a different thread. - // configureSparkMax(() -> motor.setCANTimeout(0)); // Commented it out because it prevents feedback. - } - - /** - * Initialize the {@link SwerveMotor} as a {@link CANSparkMax} connected to a Brushless Motor. - * - * @param id CAN ID of the SparkMax. - * @param isDriveMotor Is the motor being initialized a drive motor? - * @param encoderType {@link Type} of encoder to use for the {@link CANSparkMax} device. - * @param countsPerRevolution The number of encoder pulses for the {@link Type} encoder per revolution. - * @param useDataPortQuadEncoder Use the encoder attached to the data port of the spark max for a quadrature encoder. - */ - public SparkMaxBrushedMotorSwerve(int id, boolean isDriveMotor, Type encoderType, int countsPerRevolution, - boolean useDataPortQuadEncoder) - { - this(new CANSparkMax(id, MotorType.kBrushed), isDriveMotor, encoderType, countsPerRevolution, - useDataPortQuadEncoder); - } - - /** - * Run the configuration until it succeeds or times out. - * - * @param config Lambda supplier returning the error state. - */ - private void configureSparkMax(Supplier config) - { - for (int i = 0; i < maximumRetries; i++) - { - if (config.get() == REVLibError.kOk) - { - return; - } - } - DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true); - } - - /** - * Set the voltage compensation for the swerve module motor. - * - * @param nominalVoltage Nominal voltage for operation to output to. - */ - @Override - public void setVoltageCompensation(double nominalVoltage) - { - configureSparkMax(() -> motor.enableVoltageCompensation(nominalVoltage)); - } - - /** - * Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with - * voltage compensation. This is useful to protect the motor from current spikes. - * - * @param currentLimit Current limit in AMPS at free speed. - */ - @Override - public void setCurrentLimit(int currentLimit) - { - configureSparkMax(() -> motor.setSmartCurrentLimit(currentLimit)); - } - - /** - * Set the maximum rate the open/closed loop output can change by. - * - * @param rampRate Time in seconds to go from 0 to full throttle. - */ - @Override - public void setLoopRampRate(double rampRate) - { - configureSparkMax(() -> motor.setOpenLoopRampRate(rampRate)); - configureSparkMax(() -> motor.setClosedLoopRampRate(rampRate)); - } - - /** - * Get the motor object from the module. - * - * @return Motor object. - */ - @Override - public Object getMotor() - { - return motor; - } - - /** - * Queries whether the absolute encoder is directly attached to the motor controller. - * - * @return connected absolute encoder state. - */ - @Override - public boolean isAttachedAbsoluteEncoder() - { - return absoluteEncoder != null; - } - - /** - * Configure the factory defaults. - */ - @Override - public void factoryDefaults() - { - if (!factoryDefaultOccurred) - { - configureSparkMax(motor::restoreFactoryDefaults); - factoryDefaultOccurred = true; - } - } - - /** - * Clear the sticky faults on the motor controller. - */ - @Override - public void clearStickyFaults() - { - configureSparkMax(motor::clearFaults); - } - - /** - * Set the absolute encoder to be a compatible absolute encoder. - * - * @param encoder The encoder to use. - * @return The {@link SwerveMotor} for easy instantiation. - */ - @Override - public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) - { - if (encoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) - { - absoluteEncoder = (AbsoluteEncoder) encoder.getAbsoluteEncoder(); - configureSparkMax(() -> pid.setFeedbackDevice(absoluteEncoder)); - } - if (absoluteEncoder == null && this.encoder == null) - { - DriverStation.reportError("An encoder MUST be defined to work with a SparkMAX", true); - throw new RuntimeException("An encoder MUST be defined to work with a SparkMAX"); - } - return this; - } - - /** - * Configure the integrated encoder for the swerve module. Sets the conversion factors for position and velocity. - * - * @param positionConversionFactor The conversion factor to apply. - */ - @Override - public void configureIntegratedEncoder(double positionConversionFactor) - { - if (absoluteEncoder == null) - { - configureSparkMax(() -> encoder.setPositionConversionFactor(positionConversionFactor)); - configureSparkMax(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60)); - - // Taken from - // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67 - configureCANStatusFrames(10, 20, 20, 500, 500); - } else - { - configureSparkMax(() -> absoluteEncoder.setPositionConversionFactor(positionConversionFactor)); - configureSparkMax(() -> absoluteEncoder.setVelocityConversionFactor(positionConversionFactor / 60)); - } - } - - /** - * Configure the PIDF values for the closed loop controller. - * - * @param config Configuration class holding the PIDF values. - */ - @Override - public void configurePIDF(PIDFConfig config) - { -// int pidSlot = -// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal(); - int pidSlot = 0; - - configureSparkMax(() -> pid.setP(config.p, pidSlot)); - configureSparkMax(() -> pid.setI(config.i, pidSlot)); - configureSparkMax(() -> pid.setD(config.d, pidSlot)); - configureSparkMax(() -> pid.setFF(config.f, pidSlot)); - configureSparkMax(() -> pid.setIZone(config.iz, pidSlot)); - configureSparkMax(() -> pid.setOutputRange(config.output.min, config.output.max, pidSlot)); - } - - /** - * Configure the PID wrapping for the position closed loop controller. - * - * @param minInput Minimum PID input. - * @param maxInput Maximum PID input. - */ - @Override - public void configurePIDWrapping(double minInput, double maxInput) - { - configureSparkMax(() -> pid.setPositionPIDWrappingEnabled(true)); - configureSparkMax(() -> pid.setPositionPIDWrappingMinInput(minInput)); - configureSparkMax(() -> pid.setPositionPIDWrappingMaxInput(maxInput)); - } - - /** - * Set the CAN status frames. - * - * @param CANStatus0 Applied Output, Faults, Sticky Faults, Is Follower - * @param CANStatus1 Motor Velocity, Motor Temperature, Motor Voltage, Motor Current - * @param CANStatus2 Motor Position - * @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position - * @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position - */ - public void configureCANStatusFrames( - int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4) - { - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4)); - // TODO: Configure Status Frame 5 and 6 if necessary - // https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces - } - - /** - * Set the idle mode. - * - * @param isBrakeMode Set the brake mode. - */ - @Override - public void setMotorBrake(boolean isBrakeMode) - { - configureSparkMax(() -> motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast)); - } - - /** - * Set the motor to be inverted. - * - * @param inverted State of inversion. - */ - @Override - public void setInverted(boolean inverted) - { - motor.setInverted(inverted); - } - - /** - * Save the configurations from flash to EEPROM. - */ - @Override - public void burnFlash() - { - configureSparkMax(() -> motor.burnFlash()); - } - - /** - * Set the percentage output. - * - * @param percentOutput percent out for the motor controller. - */ - @Override - public void set(double percentOutput) - { - motor.set(percentOutput); - } - - /** - * Set the closed loop PID controller reference point. - * - * @param setpoint Setpoint in MPS or Angle in degrees. - * @param feedforward Feedforward in volt-meter-per-second or kV. - */ - @Override - public void setReference(double setpoint, double feedforward) - { -// int pidSlot = -// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal(); - int pidSlot = 0; - configureSparkMax(() -> - pid.setReference( - setpoint, - isDriveMotor ? ControlType.kVelocity : ControlType.kPosition, - pidSlot, - feedforward) - ); - } - - /** - * Set the closed loop PID controller reference point. - * - * @param setpoint Setpoint in meters per second or angle in degrees. - * @param feedforward Feedforward in volt-meter-per-second or kV. - * @param position Only used on the angle motor, the position of the motor in degrees. - */ - @Override - public void setReference(double setpoint, double feedforward, double position) - { - setReference(setpoint, feedforward); - } - - /** - * Get the velocity of the integrated encoder. - * - * @return velocity - */ - @Override - public double getVelocity() - { - return absoluteEncoder == null ? encoder.getVelocity() : absoluteEncoder.getVelocity(); - } - - /** - * Get the position of the integrated encoder. - * - * @return Position - */ - @Override - public double getPosition() - { - return absoluteEncoder == null ? encoder.getPosition() : absoluteEncoder.getPosition(); - } - - /** - * Set the integrated encoder position. - * - * @param position Integrated encoder position. - */ - @Override - public void setPosition(double position) - { - if (absoluteEncoder == null) - { - configureSparkMax(() -> encoder.setPosition(position)); - } - } +/** Brushed motor control with SparkMax. */ +public class SparkMaxBrushedMotorSwerve extends SwerveMotor { + + /** SparkMAX Instance. */ + public CANSparkMax motor; + + /** Absolute encoder attached to the SparkMax (if exists) */ + public AbsoluteEncoder absoluteEncoder; + /** Integrated encoder. */ + public RelativeEncoder encoder; + /** Closed-loop PID controller. */ + public SparkPIDController pid; + /** Factory default already occurred. */ + private boolean factoryDefaultOccurred = false; + + /** + * Initialize the swerve motor. + * + * @param motor The SwerveMotor as a SparkMax object. + * @param isDriveMotor Is the motor being initialized a drive motor? + * @param encoderType {@link Type} of encoder to use for the {@link CANSparkMax} device. + * @param countsPerRevolution The number of encoder pulses for the {@link Type} encoder per + * revolution. + * @param useDataPortQuadEncoder Use the encoder attached to the data port of the spark max for a + * quadrature encoder. + */ + public SparkMaxBrushedMotorSwerve( + CANSparkMax motor, + boolean isDriveMotor, + Type encoderType, + int countsPerRevolution, + boolean useDataPortQuadEncoder) { + // Drive motors **MUST** have an encoder attached. + if (isDriveMotor && encoderType == Type.kNoSensor) { + DriverStation.reportError("Cannot use motor without encoder.", true); + throw new RuntimeException( + "Cannot use SparkMAX as a drive motor without an encoder attached."); + } + + // Hall encoders can be used as quadrature encoders. + if (encoderType == Type.kHallSensor) { + encoderType = Type.kQuadrature; + } + + this.motor = motor; + this.isDriveMotor = isDriveMotor; + + factoryDefaults(); + clearStickyFaults(); + + // Get the onboard PID controller. + pid = motor.getPIDController(); + + // If there is a sensor attached to the data port or encoder port set the relative encoder. + if (isDriveMotor || (encoderType != Type.kNoSensor || useDataPortQuadEncoder)) { + this.encoder = + useDataPortQuadEncoder + ? motor.getAlternateEncoder( + SparkMaxAlternateEncoder.Type.kQuadrature, countsPerRevolution) + : motor.getEncoder(encoderType, countsPerRevolution); + + // Configure feedback of the PID controller as the integrated encoder. + configureSparkMax(() -> pid.setFeedbackDevice(encoder)); + } + // Spin off configurations in a different thread. + // configureSparkMax(() -> motor.setCANTimeout(0)); // Commented it out because it prevents + // feedback. + } + + /** + * Initialize the {@link SwerveMotor} as a {@link CANSparkMax} connected to a Brushless Motor. + * + * @param id CAN ID of the SparkMax. + * @param isDriveMotor Is the motor being initialized a drive motor? + * @param encoderType {@link Type} of encoder to use for the {@link CANSparkMax} device. + * @param countsPerRevolution The number of encoder pulses for the {@link Type} encoder per + * revolution. + * @param useDataPortQuadEncoder Use the encoder attached to the data port of the spark max for a + * quadrature encoder. + */ + public SparkMaxBrushedMotorSwerve( + int id, + boolean isDriveMotor, + Type encoderType, + int countsPerRevolution, + boolean useDataPortQuadEncoder) { + this( + new CANSparkMax(id, MotorType.kBrushed), + isDriveMotor, + encoderType, + countsPerRevolution, + useDataPortQuadEncoder); + } + + /** + * Run the configuration until it succeeds or times out. + * + * @param config Lambda supplier returning the error state. + */ + private void configureSparkMax(Supplier config) { + for (int i = 0; i < maximumRetries; i++) { + if (config.get() == REVLibError.kOk) { + return; + } + } + DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true); + } + + /** + * Set the voltage compensation for the swerve module motor. + * + * @param nominalVoltage Nominal voltage for operation to output to. + */ + @Override + public void setVoltageCompensation(double nominalVoltage) { + configureSparkMax(() -> motor.enableVoltageCompensation(nominalVoltage)); + } + + /** + * Set the current limit for the swerve drive motor, remember this may cause jumping if used in + * conjunction with voltage compensation. This is useful to protect the motor from current spikes. + * + * @param currentLimit Current limit in AMPS at free speed. + */ + @Override + public void setCurrentLimit(int currentLimit) { + configureSparkMax(() -> motor.setSmartCurrentLimit(currentLimit)); + } + + /** + * Set the maximum rate the open/closed loop output can change by. + * + * @param rampRate Time in seconds to go from 0 to full throttle. + */ + @Override + public void setLoopRampRate(double rampRate) { + configureSparkMax(() -> motor.setOpenLoopRampRate(rampRate)); + configureSparkMax(() -> motor.setClosedLoopRampRate(rampRate)); + } + + /** + * Get the motor object from the module. + * + * @return Motor object. + */ + @Override + public Object getMotor() { + return motor; + } + + /** + * Queries whether the absolute encoder is directly attached to the motor controller. + * + * @return connected absolute encoder state. + */ + @Override + public boolean isAttachedAbsoluteEncoder() { + return absoluteEncoder != null; + } + + /** Configure the factory defaults. */ + @Override + public void factoryDefaults() { + if (!factoryDefaultOccurred) { + configureSparkMax(motor::restoreFactoryDefaults); + factoryDefaultOccurred = true; + } + } + + /** Clear the sticky faults on the motor controller. */ + @Override + public void clearStickyFaults() { + configureSparkMax(motor::clearFaults); + } + + /** + * Set the absolute encoder to be a compatible absolute encoder. + * + * @param encoder The encoder to use. + * @return The {@link SwerveMotor} for easy instantiation. + */ + @Override + public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) { + if (encoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) { + absoluteEncoder = (AbsoluteEncoder) encoder.getAbsoluteEncoder(); + configureSparkMax(() -> pid.setFeedbackDevice(absoluteEncoder)); + } + if (absoluteEncoder == null && this.encoder == null) { + DriverStation.reportError("An encoder MUST be defined to work with a SparkMAX", true); + throw new RuntimeException("An encoder MUST be defined to work with a SparkMAX"); + } + return this; + } + + /** + * Configure the integrated encoder for the swerve module. Sets the conversion factors for + * position and velocity. + * + * @param positionConversionFactor The conversion factor to apply. + */ + @Override + public void configureIntegratedEncoder(double positionConversionFactor) { + if (absoluteEncoder == null) { + configureSparkMax(() -> encoder.setPositionConversionFactor(positionConversionFactor)); + configureSparkMax(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60)); + + // Taken from + // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67 + configureCANStatusFrames(10, 20, 20, 500, 500); + } else { + configureSparkMax( + () -> absoluteEncoder.setPositionConversionFactor(positionConversionFactor)); + configureSparkMax( + () -> absoluteEncoder.setVelocityConversionFactor(positionConversionFactor / 60)); + } + } + + /** + * Configure the PIDF values for the closed loop controller. + * + * @param config Configuration class holding the PIDF values. + */ + @Override + public void configurePIDF(PIDFConfig config) { + // int pidSlot = + // isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : + // SparkMAX_slotIdx.Position.ordinal(); + int pidSlot = 0; + + configureSparkMax(() -> pid.setP(config.p, pidSlot)); + configureSparkMax(() -> pid.setI(config.i, pidSlot)); + configureSparkMax(() -> pid.setD(config.d, pidSlot)); + configureSparkMax(() -> pid.setFF(config.f, pidSlot)); + configureSparkMax(() -> pid.setIZone(config.iz, pidSlot)); + configureSparkMax(() -> pid.setOutputRange(config.output.min, config.output.max, pidSlot)); + } + + /** + * Configure the PID wrapping for the position closed loop controller. + * + * @param minInput Minimum PID input. + * @param maxInput Maximum PID input. + */ + @Override + public void configurePIDWrapping(double minInput, double maxInput) { + configureSparkMax(() -> pid.setPositionPIDWrappingEnabled(true)); + configureSparkMax(() -> pid.setPositionPIDWrappingMinInput(minInput)); + configureSparkMax(() -> pid.setPositionPIDWrappingMaxInput(maxInput)); + } + + /** + * Set the CAN status frames. + * + * @param CANStatus0 Applied Output, Faults, Sticky Faults, Is Follower + * @param CANStatus1 Motor Velocity, Motor Temperature, Motor Voltage, Motor Current + * @param CANStatus2 Motor Position + * @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position + * @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position + */ + public void configureCANStatusFrames( + int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4) { + configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0)); + configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1)); + configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2)); + configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3)); + configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4)); + // TODO: Configure Status Frame 5 and 6 if necessary + // https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces + } + + /** + * Set the idle mode. + * + * @param isBrakeMode Set the brake mode. + */ + @Override + public void setMotorBrake(boolean isBrakeMode) { + configureSparkMax(() -> motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast)); + } + + /** + * Set the motor to be inverted. + * + * @param inverted State of inversion. + */ + @Override + public void setInverted(boolean inverted) { + motor.setInverted(inverted); + } + + /** Save the configurations from flash to EEPROM. */ + @Override + public void burnFlash() { + configureSparkMax(() -> motor.burnFlash()); + } + + /** + * Set the percentage output. + * + * @param percentOutput percent out for the motor controller. + */ + @Override + public void set(double percentOutput) { + motor.set(percentOutput); + } + + /** + * Set the closed loop PID controller reference point. + * + * @param setpoint Setpoint in MPS or Angle in degrees. + * @param feedforward Feedforward in volt-meter-per-second or kV. + */ + @Override + public void setReference(double setpoint, double feedforward) { + // int pidSlot = + // isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : + // SparkMAX_slotIdx.Position.ordinal(); + int pidSlot = 0; + configureSparkMax( + () -> + pid.setReference( + setpoint, + isDriveMotor ? ControlType.kVelocity : ControlType.kPosition, + pidSlot, + feedforward)); + } + + /** + * Set the closed loop PID controller reference point. + * + * @param setpoint Setpoint in meters per second or angle in degrees. + * @param feedforward Feedforward in volt-meter-per-second or kV. + * @param position Only used on the angle motor, the position of the motor in degrees. + */ + @Override + public void setReference(double setpoint, double feedforward, double position) { + setReference(setpoint, feedforward); + } + + /** + * Get the velocity of the integrated encoder. + * + * @return velocity + */ + @Override + public double getVelocity() { + return absoluteEncoder == null ? encoder.getVelocity() : absoluteEncoder.getVelocity(); + } + + /** + * Get the position of the integrated encoder. + * + * @return Position + */ + @Override + public double getPosition() { + return absoluteEncoder == null ? encoder.getPosition() : absoluteEncoder.getPosition(); + } + + /** + * Set the integrated encoder position. + * + * @param position Integrated encoder position. + */ + @Override + public void setPosition(double position) { + if (absoluteEncoder == null) { + configureSparkMax(() -> encoder.setPosition(position)); + } + } } diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java index 50b10ae7..98503854 100644 --- a/src/main/java/swervelib/motors/SparkMaxSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java @@ -1,11 +1,11 @@ package swervelib.motors; import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkLowLevel.PeriodicFrame; +import com.revrobotics.CANSparkMax; import com.revrobotics.MotorFeedbackSensor; import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; @@ -17,430 +17,361 @@ import swervelib.parser.PIDFConfig; import swervelib.telemetry.SwerveDriveTelemetry; -/** - * An implementation of {@link CANSparkMax} as a {@link SwerveMotor}. - */ -public class SparkMaxSwerve extends SwerveMotor -{ - - /** - * SparkMAX Instance. - */ - public CANSparkMax motor; - /** - * Integrated encoder. - */ - public RelativeEncoder encoder; - /** - * Absolute encoder attached to the SparkMax (if exists) - */ - public SwerveAbsoluteEncoder absoluteEncoder; - /** - * Closed-loop PID controller. - */ - public SparkPIDController pid; - /** - * Factory default already occurred. - */ - private boolean factoryDefaultOccurred = false; - - /** - * Initialize the swerve motor. - * - * @param motor The SwerveMotor as a SparkMax object. - * @param isDriveMotor Is the motor being initialized a drive motor? - */ - public SparkMaxSwerve(CANSparkMax motor, boolean isDriveMotor) - { - this.motor = motor; - this.isDriveMotor = isDriveMotor; - factoryDefaults(); - clearStickyFaults(); - - encoder = motor.getEncoder(); - pid = motor.getPIDController(); - pid.setFeedbackDevice( - encoder); // Configure feedback of the PID controller as the integrated encoder. - - // Spin off configurations in a different thread. - // configureSparkMax(() -> motor.setCANTimeout(0)); // Commented out because it prevents feedback. - } - - /** - * Initialize the {@link SwerveMotor} as a {@link CANSparkMax} connected to a Brushless Motor. - * - * @param id CAN ID of the SparkMax. - * @param isDriveMotor Is the motor being initialized a drive motor? - */ - public SparkMaxSwerve(int id, boolean isDriveMotor) - { - this(new CANSparkMax(id, MotorType.kBrushless), isDriveMotor); - } - - /** - * Run the configuration until it succeeds or times out. - * - * @param config Lambda supplier returning the error state. - */ - private void configureSparkMax(Supplier config) - { - for (int i = 0; i < maximumRetries; i++) - { - if (config.get() == REVLibError.kOk) - { - return; - } - } - DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true); - } - - /** - * Set the voltage compensation for the swerve module motor. - * - * @param nominalVoltage Nominal voltage for operation to output to. - */ - @Override - public void setVoltageCompensation(double nominalVoltage) - { - configureSparkMax(() -> motor.enableVoltageCompensation(nominalVoltage)); - } - - /** - * Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with - * voltage compensation. This is useful to protect the motor from current spikes. - * - * @param currentLimit Current limit in AMPS at free speed. - */ - @Override - public void setCurrentLimit(int currentLimit) - { - configureSparkMax(() -> motor.setSmartCurrentLimit(currentLimit)); - } - - /** - * Set the maximum rate the open/closed loop output can change by. - * - * @param rampRate Time in seconds to go from 0 to full throttle. - */ - @Override - public void setLoopRampRate(double rampRate) - { - configureSparkMax(() -> motor.setOpenLoopRampRate(rampRate)); - configureSparkMax(() -> motor.setClosedLoopRampRate(rampRate)); - } - - /** - * Get the motor object from the module. - * - * @return Motor object. - */ - @Override - public Object getMotor() - { - return motor; - } - - /** - * Queries whether the absolute encoder is directly attached to the motor controller. - * - * @return connected absolute encoder state. - */ - @Override - public boolean isAttachedAbsoluteEncoder() - { - return absoluteEncoder != null; - } - - /** - * Configure the factory defaults. - */ - @Override - public void factoryDefaults() - { - if (!factoryDefaultOccurred) - { - configureSparkMax(motor::restoreFactoryDefaults); - factoryDefaultOccurred = true; - } - } - - /** - * Clear the sticky faults on the motor controller. - */ - @Override - public void clearStickyFaults() - { - configureSparkMax(motor::clearFaults); - } - - /** - * Set the absolute encoder to be a compatible absolute encoder. - * - * @param encoder The encoder to use. - * @return The {@link SwerveMotor} for easy instantiation. - */ - @Override - public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) - { - if (encoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) - { - DriverStation.reportWarning( - "IF possible configure the duty cycle encoder offset in the REV Hardware Client instead of using the" + - " absoluteEncoderOffset in the Swerve Module JSON!", - false); - absoluteEncoder = encoder; - configureSparkMax(() -> pid.setFeedbackDevice((MotorFeedbackSensor) absoluteEncoder.getAbsoluteEncoder())); - } - return this; - } - - /** - * Configure the integrated encoder for the swerve module. Sets the conversion factors for position and velocity. - * - * @param positionConversionFactor The conversion factor to apply. - */ - @Override - public void configureIntegratedEncoder(double positionConversionFactor) - { - if (absoluteEncoder == null) - { - configureSparkMax(() -> encoder.setPositionConversionFactor(positionConversionFactor)); - configureSparkMax(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60)); - - // Taken from - // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67 - configureCANStatusFrames(10, 20, 20, 500, 500); - } else - { - configureSparkMax(() -> { - if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) - { - return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor( - positionConversionFactor); - } else - { - return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor( - positionConversionFactor); - } - }); - configureSparkMax(() -> { - if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) - { - return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor( - positionConversionFactor / 60); - } else - { - return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor( - positionConversionFactor / 60); - } - }); - } - } - - /** - * Configure the PIDF values for the closed loop controller. - * - * @param config Configuration class holding the PIDF values. - */ - @Override - public void configurePIDF(PIDFConfig config) - { -// int pidSlot = -// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal(); - int pidSlot = 0; - configureSparkMax(() -> pid.setP(config.p, pidSlot)); - configureSparkMax(() -> pid.setI(config.i, pidSlot)); - configureSparkMax(() -> pid.setD(config.d, pidSlot)); - configureSparkMax(() -> pid.setFF(config.f, pidSlot)); - configureSparkMax(() -> pid.setIZone(config.iz, pidSlot)); - configureSparkMax(() -> pid.setOutputRange(config.output.min, config.output.max, pidSlot)); - } - - /** - * Configure the PID wrapping for the position closed loop controller. - * - * @param minInput Minimum PID input. - * @param maxInput Maximum PID input. - */ - @Override - public void configurePIDWrapping(double minInput, double maxInput) - { - configureSparkMax(() -> pid.setPositionPIDWrappingEnabled(true)); - configureSparkMax(() -> pid.setPositionPIDWrappingMinInput(minInput)); - configureSparkMax(() -> pid.setPositionPIDWrappingMaxInput(maxInput)); - } - - /** - * Set the CAN status frames. - * - * @param CANStatus0 Applied Output, Faults, Sticky Faults, Is Follower - * @param CANStatus1 Motor Velocity, Motor Temperature, Motor Voltage, Motor Current - * @param CANStatus2 Motor Position - * @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position - * @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position - */ - public void configureCANStatusFrames( - int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4) - { - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4)); - // TODO: Configure Status Frame 5 and 6 if necessary - // https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces - } - - /** - * Set the idle mode. - * - * @param isBrakeMode Set the brake mode. - */ - @Override - public void setMotorBrake(boolean isBrakeMode) - { - configureSparkMax(() -> motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast)); - } - - /** - * Set the motor to be inverted. - * - * @param inverted State of inversion. - */ - @Override - public void setInverted(boolean inverted) - { - motor.setInverted(inverted); - } - - /** - * Save the configurations from flash to EEPROM. - */ - @Override - public void burnFlash() - { - try - { - Thread.sleep(200); - } catch (Exception e) - { - } - configureSparkMax(() -> motor.burnFlash()); - } - - /** - * Set the percentage output. - * - * @param percentOutput percent out for the motor controller. - */ - @Override - public void set(double percentOutput) - { - motor.set(percentOutput); - } - - /** - * Set the closed loop PID controller reference point. - * - * @param setpoint Setpoint in MPS or Angle in degrees. - * @param feedforward Feedforward in volt-meter-per-second or kV. - */ - @Override - public void setReference(double setpoint, double feedforward) - { - boolean possibleBurnOutIssue = true; -// int pidSlot = -// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal(); - int pidSlot = 0; - - if (isDriveMotor) - { - configureSparkMax(() -> - pid.setReference( - setpoint, - ControlType.kVelocity, - pidSlot, - feedforward)); - } else - { - configureSparkMax(() -> - pid.setReference( - setpoint, - ControlType.kPosition, - pidSlot, - feedforward)); - if(SwerveDriveTelemetry.isSimulation) - { - encoder.setPosition(setpoint); - } - } - } - - /** - * Set the closed loop PID controller reference point. - * - * @param setpoint Setpoint in meters per second or angle in degrees. - * @param feedforward Feedforward in volt-meter-per-second or kV. - * @param position Only used on the angle motor, the position of the motor in degrees. - */ - @Override - public void setReference(double setpoint, double feedforward, double position) - { - setReference(setpoint, feedforward); - } - - /** - * Get the velocity of the integrated encoder. - * - * @return velocity - */ - @Override - public double getVelocity() - { - return absoluteEncoder == null ? encoder.getVelocity() : absoluteEncoder.getVelocity(); - } - - /** - * Get the position of the integrated encoder. - * - * @return Position - */ - @Override - public double getPosition() - { - return absoluteEncoder == null ? encoder.getPosition() : absoluteEncoder.getAbsolutePosition(); - } - - /** - * Set the integrated encoder position. - * - * @param position Integrated encoder position. - */ - @Override - public void setPosition(double position) - { - if (absoluteEncoder == null) - { - configureSparkMax(() -> encoder.setPosition(position)); - } - } - - /** - * REV Slots for PID configuration. - */ - enum SparkMAX_slotIdx - { - /** - * Slot 1, used for position PID's. - */ - Position, - /** - * Slot 2, used for velocity PID's. - */ - Velocity, - /** - * Slot 3, used arbitrarily. (Documentation show simulations). - */ - Simulation - } +/** An implementation of {@link CANSparkMax} as a {@link SwerveMotor}. */ +public class SparkMaxSwerve extends SwerveMotor { + + /** SparkMAX Instance. */ + public CANSparkMax motor; + /** Integrated encoder. */ + public RelativeEncoder encoder; + /** Absolute encoder attached to the SparkMax (if exists) */ + public SwerveAbsoluteEncoder absoluteEncoder; + /** Closed-loop PID controller. */ + public SparkPIDController pid; + /** Factory default already occurred. */ + private boolean factoryDefaultOccurred = false; + + /** + * Initialize the swerve motor. + * + * @param motor The SwerveMotor as a SparkMax object. + * @param isDriveMotor Is the motor being initialized a drive motor? + */ + public SparkMaxSwerve(CANSparkMax motor, boolean isDriveMotor) { + this.motor = motor; + this.isDriveMotor = isDriveMotor; + factoryDefaults(); + clearStickyFaults(); + + encoder = motor.getEncoder(); + pid = motor.getPIDController(); + pid.setFeedbackDevice( + encoder); // Configure feedback of the PID controller as the integrated encoder. + + // Spin off configurations in a different thread. + // configureSparkMax(() -> motor.setCANTimeout(0)); // Commented out because it prevents + // feedback. + } + + /** + * Initialize the {@link SwerveMotor} as a {@link CANSparkMax} connected to a Brushless Motor. + * + * @param id CAN ID of the SparkMax. + * @param isDriveMotor Is the motor being initialized a drive motor? + */ + public SparkMaxSwerve(int id, boolean isDriveMotor) { + this(new CANSparkMax(id, MotorType.kBrushless), isDriveMotor); + } + + /** + * Run the configuration until it succeeds or times out. + * + * @param config Lambda supplier returning the error state. + */ + private void configureSparkMax(Supplier config) { + for (int i = 0; i < maximumRetries; i++) { + if (config.get() == REVLibError.kOk) { + return; + } + } + DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true); + } + + /** + * Set the voltage compensation for the swerve module motor. + * + * @param nominalVoltage Nominal voltage for operation to output to. + */ + @Override + public void setVoltageCompensation(double nominalVoltage) { + configureSparkMax(() -> motor.enableVoltageCompensation(nominalVoltage)); + } + + /** + * Set the current limit for the swerve drive motor, remember this may cause jumping if used in + * conjunction with voltage compensation. This is useful to protect the motor from current spikes. + * + * @param currentLimit Current limit in AMPS at free speed. + */ + @Override + public void setCurrentLimit(int currentLimit) { + configureSparkMax(() -> motor.setSmartCurrentLimit(currentLimit)); + } + + /** + * Set the maximum rate the open/closed loop output can change by. + * + * @param rampRate Time in seconds to go from 0 to full throttle. + */ + @Override + public void setLoopRampRate(double rampRate) { + configureSparkMax(() -> motor.setOpenLoopRampRate(rampRate)); + configureSparkMax(() -> motor.setClosedLoopRampRate(rampRate)); + } + + /** + * Get the motor object from the module. + * + * @return Motor object. + */ + @Override + public Object getMotor() { + return motor; + } + + /** + * Queries whether the absolute encoder is directly attached to the motor controller. + * + * @return connected absolute encoder state. + */ + @Override + public boolean isAttachedAbsoluteEncoder() { + return absoluteEncoder != null; + } + + /** Configure the factory defaults. */ + @Override + public void factoryDefaults() { + if (!factoryDefaultOccurred) { + configureSparkMax(motor::restoreFactoryDefaults); + factoryDefaultOccurred = true; + } + } + + /** Clear the sticky faults on the motor controller. */ + @Override + public void clearStickyFaults() { + configureSparkMax(motor::clearFaults); + } + + /** + * Set the absolute encoder to be a compatible absolute encoder. + * + * @param encoder The encoder to use. + * @return The {@link SwerveMotor} for easy instantiation. + */ + @Override + public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) { + if (encoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) { + DriverStation.reportWarning( + "IF possible configure the duty cycle encoder offset in the REV Hardware Client instead of using the" + + " absoluteEncoderOffset in the Swerve Module JSON!", + false); + absoluteEncoder = encoder; + configureSparkMax( + () -> pid.setFeedbackDevice((MotorFeedbackSensor) absoluteEncoder.getAbsoluteEncoder())); + } + return this; + } + + /** + * Configure the integrated encoder for the swerve module. Sets the conversion factors for + * position and velocity. + * + * @param positionConversionFactor The conversion factor to apply. + */ + @Override + public void configureIntegratedEncoder(double positionConversionFactor) { + if (absoluteEncoder == null) { + configureSparkMax(() -> encoder.setPositionConversionFactor(positionConversionFactor)); + configureSparkMax(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60)); + + // Taken from + // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67 + configureCANStatusFrames(10, 20, 20, 500, 500); + } else { + configureSparkMax( + () -> { + if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) { + return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()) + .setPositionConversionFactor(positionConversionFactor); + } else { + return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()) + .setPositionConversionFactor(positionConversionFactor); + } + }); + configureSparkMax( + () -> { + if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) { + return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()) + .setVelocityConversionFactor(positionConversionFactor / 60); + } else { + return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()) + .setVelocityConversionFactor(positionConversionFactor / 60); + } + }); + } + } + + /** + * Configure the PIDF values for the closed loop controller. + * + * @param config Configuration class holding the PIDF values. + */ + @Override + public void configurePIDF(PIDFConfig config) { + // int pidSlot = + // isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : + // SparkMAX_slotIdx.Position.ordinal(); + int pidSlot = 0; + configureSparkMax(() -> pid.setP(config.p, pidSlot)); + configureSparkMax(() -> pid.setI(config.i, pidSlot)); + configureSparkMax(() -> pid.setD(config.d, pidSlot)); + configureSparkMax(() -> pid.setFF(config.f, pidSlot)); + configureSparkMax(() -> pid.setIZone(config.iz, pidSlot)); + configureSparkMax(() -> pid.setOutputRange(config.output.min, config.output.max, pidSlot)); + } + + /** + * Configure the PID wrapping for the position closed loop controller. + * + * @param minInput Minimum PID input. + * @param maxInput Maximum PID input. + */ + @Override + public void configurePIDWrapping(double minInput, double maxInput) { + configureSparkMax(() -> pid.setPositionPIDWrappingEnabled(true)); + configureSparkMax(() -> pid.setPositionPIDWrappingMinInput(minInput)); + configureSparkMax(() -> pid.setPositionPIDWrappingMaxInput(maxInput)); + } + + /** + * Set the CAN status frames. + * + * @param CANStatus0 Applied Output, Faults, Sticky Faults, Is Follower + * @param CANStatus1 Motor Velocity, Motor Temperature, Motor Voltage, Motor Current + * @param CANStatus2 Motor Position + * @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position + * @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position + */ + public void configureCANStatusFrames( + int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4) { + configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0)); + configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1)); + configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2)); + configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3)); + configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4)); + // TODO: Configure Status Frame 5 and 6 if necessary + // https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces + } + + /** + * Set the idle mode. + * + * @param isBrakeMode Set the brake mode. + */ + @Override + public void setMotorBrake(boolean isBrakeMode) { + configureSparkMax(() -> motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast)); + } + + /** + * Set the motor to be inverted. + * + * @param inverted State of inversion. + */ + @Override + public void setInverted(boolean inverted) { + motor.setInverted(inverted); + } + + /** Save the configurations from flash to EEPROM. */ + @Override + public void burnFlash() { + try { + Thread.sleep(200); + } catch (Exception e) { + } + configureSparkMax(() -> motor.burnFlash()); + } + + /** + * Set the percentage output. + * + * @param percentOutput percent out for the motor controller. + */ + @Override + public void set(double percentOutput) { + motor.set(percentOutput); + } + + /** + * Set the closed loop PID controller reference point. + * + * @param setpoint Setpoint in MPS or Angle in degrees. + * @param feedforward Feedforward in volt-meter-per-second or kV. + */ + @Override + public void setReference(double setpoint, double feedforward) { + boolean possibleBurnOutIssue = true; + // int pidSlot = + // isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : + // SparkMAX_slotIdx.Position.ordinal(); + int pidSlot = 0; + + if (isDriveMotor) { + configureSparkMax( + () -> pid.setReference(setpoint, ControlType.kVelocity, pidSlot, feedforward)); + } else { + configureSparkMax( + () -> pid.setReference(setpoint, ControlType.kPosition, pidSlot, feedforward)); + if (SwerveDriveTelemetry.isSimulation) { + encoder.setPosition(setpoint); + } + } + } + + /** + * Set the closed loop PID controller reference point. + * + * @param setpoint Setpoint in meters per second or angle in degrees. + * @param feedforward Feedforward in volt-meter-per-second or kV. + * @param position Only used on the angle motor, the position of the motor in degrees. + */ + @Override + public void setReference(double setpoint, double feedforward, double position) { + setReference(setpoint, feedforward); + } + + /** + * Get the velocity of the integrated encoder. + * + * @return velocity + */ + @Override + public double getVelocity() { + return absoluteEncoder == null ? encoder.getVelocity() : absoluteEncoder.getVelocity(); + } + + /** + * Get the position of the integrated encoder. + * + * @return Position + */ + @Override + public double getPosition() { + return absoluteEncoder == null ? encoder.getPosition() : absoluteEncoder.getAbsolutePosition(); + } + + /** + * Set the integrated encoder position. + * + * @param position Integrated encoder position. + */ + @Override + public void setPosition(double position) { + if (absoluteEncoder == null) { + configureSparkMax(() -> encoder.setPosition(position)); + } + } + + /** REV Slots for PID configuration. */ + enum SparkMAX_slotIdx { + /** Slot 1, used for position PID's. */ + Position, + /** Slot 2, used for velocity PID's. */ + Velocity, + /** Slot 3, used arbitrarily. (Documentation show simulations). */ + Simulation + } } diff --git a/src/main/java/swervelib/motors/SwerveMotor.java b/src/main/java/swervelib/motors/SwerveMotor.java index 7d585cac..5efa3deb 100644 --- a/src/main/java/swervelib/motors/SwerveMotor.java +++ b/src/main/java/swervelib/motors/SwerveMotor.java @@ -6,155 +6,148 @@ /** * Swerve motor abstraction which defines a standard interface for motors within a swerve module. */ -public abstract class SwerveMotor -{ - - /** - * The maximum amount of times the swerve motor will attempt to configure a motor if failures occur. - */ - public final int maximumRetries = 5; - /** - * Whether the swerve motor is a drive motor. - */ - protected boolean isDriveMotor; - - /** - * Configure the factory defaults. - */ - public abstract void factoryDefaults(); - - /** - * Clear the sticky faults on the motor controller. - */ - public abstract void clearStickyFaults(); - - /** - * Set the absolute encoder to be a compatible absolute encoder. - * - * @param encoder The encoder to use. - * @return The {@link SwerveMotor} for single line configuration. - */ - public abstract SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder); - - /** - * Configure the integrated encoder for the swerve module. Sets the conversion factors for position and velocity. - * - * @param positionConversionFactor The conversion factor to apply for position. - */ - public abstract void configureIntegratedEncoder(double positionConversionFactor); - - /** - * Configure the PIDF values for the closed loop controller. 0 is disabled or off. - * - * @param config Configuration class holding the PIDF values. - */ - public abstract void configurePIDF(PIDFConfig config); - - /** - * Configure the PID wrapping for the position closed loop controller. - * - * @param minInput Minimum PID input. - * @param maxInput Maximum PID input. - */ - public abstract void configurePIDWrapping(double minInput, double maxInput); - - /** - * Set the idle mode. - * - * @param isBrakeMode Set the brake mode. - */ - public abstract void setMotorBrake(boolean isBrakeMode); - - /** - * Set the motor to be inverted. - * - * @param inverted State of inversion. - */ - public abstract void setInverted(boolean inverted); - - /** - * Save the configurations from flash to EEPROM. - */ - public abstract void burnFlash(); - - /** - * Set the percentage output. - * - * @param percentOutput percent out for the motor controller. - */ - public abstract void set(double percentOutput); - - /** - * Set the closed loop PID controller reference point. - * - * @param setpoint Setpoint in meters per second or angle in degrees. - * @param feedforward Feedforward in volt-meter-per-second or kV. - */ - public abstract void setReference(double setpoint, double feedforward); - - /** - * Set the closed loop PID controller reference point. - * - * @param setpoint Setpoint in meters per second or angle in degrees. - * @param feedforward Feedforward in volt-meter-per-second or kV. - * @param position Only used on the angle motor, the position of the motor in degrees. - */ - public abstract void setReference(double setpoint, double feedforward, double position); - - /** - * Get the velocity of the integrated encoder. - * - * @return velocity in meters per second or degrees per second. - */ - public abstract double getVelocity(); - - /** - * Get the position of the integrated encoder. - * - * @return Position in meters or degrees. - */ - public abstract double getPosition(); - - /** - * Set the integrated encoder position. - * - * @param position Integrated encoder position. Should be angle in degrees or meters per second. - */ - public abstract void setPosition(double position); - - /** - * Set the voltage compensation for the swerve module motor. - * - * @param nominalVoltage Nominal voltage for operation to output to. - */ - public abstract void setVoltageCompensation(double nominalVoltage); - - /** - * Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with - * voltage compensation. This is useful to protect the motor from current spikes. - * - * @param currentLimit Current limit in AMPS at free speed. - */ - public abstract void setCurrentLimit(int currentLimit); - - /** - * Set the maximum rate the open/closed loop output can change by. - * - * @param rampRate Time in seconds to go from 0 to full throttle. - */ - public abstract void setLoopRampRate(double rampRate); - - /** - * Get the motor object from the module. - * - * @return Motor object. - */ - public abstract Object getMotor(); - - /** - * Queries whether the absolute encoder is directly attached to the motor controller. - * - * @return connected absolute encoder state. - */ - public abstract boolean isAttachedAbsoluteEncoder(); +public abstract class SwerveMotor { + + /** + * The maximum amount of times the swerve motor will attempt to configure a motor if failures + * occur. + */ + public final int maximumRetries = 5; + /** Whether the swerve motor is a drive motor. */ + protected boolean isDriveMotor; + + /** Configure the factory defaults. */ + public abstract void factoryDefaults(); + + /** Clear the sticky faults on the motor controller. */ + public abstract void clearStickyFaults(); + + /** + * Set the absolute encoder to be a compatible absolute encoder. + * + * @param encoder The encoder to use. + * @return The {@link SwerveMotor} for single line configuration. + */ + public abstract SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder); + + /** + * Configure the integrated encoder for the swerve module. Sets the conversion factors for + * position and velocity. + * + * @param positionConversionFactor The conversion factor to apply for position. + */ + public abstract void configureIntegratedEncoder(double positionConversionFactor); + + /** + * Configure the PIDF values for the closed loop controller. 0 is disabled or off. + * + * @param config Configuration class holding the PIDF values. + */ + public abstract void configurePIDF(PIDFConfig config); + + /** + * Configure the PID wrapping for the position closed loop controller. + * + * @param minInput Minimum PID input. + * @param maxInput Maximum PID input. + */ + public abstract void configurePIDWrapping(double minInput, double maxInput); + + /** + * Set the idle mode. + * + * @param isBrakeMode Set the brake mode. + */ + public abstract void setMotorBrake(boolean isBrakeMode); + + /** + * Set the motor to be inverted. + * + * @param inverted State of inversion. + */ + public abstract void setInverted(boolean inverted); + + /** Save the configurations from flash to EEPROM. */ + public abstract void burnFlash(); + + /** + * Set the percentage output. + * + * @param percentOutput percent out for the motor controller. + */ + public abstract void set(double percentOutput); + + /** + * Set the closed loop PID controller reference point. + * + * @param setpoint Setpoint in meters per second or angle in degrees. + * @param feedforward Feedforward in volt-meter-per-second or kV. + */ + public abstract void setReference(double setpoint, double feedforward); + + /** + * Set the closed loop PID controller reference point. + * + * @param setpoint Setpoint in meters per second or angle in degrees. + * @param feedforward Feedforward in volt-meter-per-second or kV. + * @param position Only used on the angle motor, the position of the motor in degrees. + */ + public abstract void setReference(double setpoint, double feedforward, double position); + + /** + * Get the velocity of the integrated encoder. + * + * @return velocity in meters per second or degrees per second. + */ + public abstract double getVelocity(); + + /** + * Get the position of the integrated encoder. + * + * @return Position in meters or degrees. + */ + public abstract double getPosition(); + + /** + * Set the integrated encoder position. + * + * @param position Integrated encoder position. Should be angle in degrees or meters per second. + */ + public abstract void setPosition(double position); + + /** + * Set the voltage compensation for the swerve module motor. + * + * @param nominalVoltage Nominal voltage for operation to output to. + */ + public abstract void setVoltageCompensation(double nominalVoltage); + + /** + * Set the current limit for the swerve drive motor, remember this may cause jumping if used in + * conjunction with voltage compensation. This is useful to protect the motor from current spikes. + * + * @param currentLimit Current limit in AMPS at free speed. + */ + public abstract void setCurrentLimit(int currentLimit); + + /** + * Set the maximum rate the open/closed loop output can change by. + * + * @param rampRate Time in seconds to go from 0 to full throttle. + */ + public abstract void setLoopRampRate(double rampRate); + + /** + * Get the motor object from the module. + * + * @return Motor object. + */ + public abstract Object getMotor(); + + /** + * Queries whether the absolute encoder is directly attached to the motor controller. + * + * @return connected absolute encoder state. + */ + public abstract boolean isAttachedAbsoluteEncoder(); } diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java index 70e98b23..b8d32151 100644 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -11,419 +11,381 @@ import swervelib.parser.PIDFConfig; import swervelib.telemetry.SwerveDriveTelemetry; -/** - * {@link com.ctre.phoenix6.hardware.TalonFX} Swerve Motor. Made by Team 1466 WebbRobotics. - */ -public class TalonFXSwerve extends SwerveMotor -{ - - /** - * Factory default already occurred. - */ - private final boolean factoryDefaultOccurred = false; - /** - * Current TalonFX configuration. - */ - private TalonFXConfiguration configuration = new TalonFXConfiguration(); - /** - * Whether the absolute encoder is integrated. - */ - private final boolean absoluteEncoder = false; - /** - * Motion magic angle voltage setter. - */ - private final MotionMagicVoltage m_angleVoltageSetter = new MotionMagicVoltage(0); -// /** -// * Motion Magic exponential voltage setters. -// */ -// private final MotionMagicExpoVoltage m_angleVoltageExpoSetter = new MotionMagicExpoVoltage(0); - /** - * Velocity voltage setter for controlling drive motor. - */ - private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0); - /** - * TalonFX motor controller. - */ - TalonFX motor; - - /** - * Constructor for TalonFX swerve motor. - * - * @param motor Motor to use. - * @param isDriveMotor Whether this motor is a drive motor. - */ - public TalonFXSwerve(TalonFX motor, boolean isDriveMotor) - { - this.isDriveMotor = isDriveMotor; - this.motor = motor; - - factoryDefaults(); - clearStickyFaults(); - -// if (SwerveDriveTelemetry.isSimulation) -// { -//// PhysicsSim.getInstance().addTalonFX(motor, .25, 6800); -// } - } - - /** - * Construct the TalonFX swerve motor given the ID and CANBus. - * - * @param id ID of the TalonFX on the CANBus. - * @param canbus CANBus on which the TalonFX is on. - * @param isDriveMotor Whether the motor is a drive or steering motor. - */ - public TalonFXSwerve(int id, String canbus, boolean isDriveMotor) - { - this(new TalonFX(id, canbus), isDriveMotor); - } - - /** - * Construct the TalonFX swerve motor given the ID. - * - * @param id ID of the TalonFX on the canbus. - * @param isDriveMotor Whether the motor is a drive or steering motor. - */ - public TalonFXSwerve(int id, boolean isDriveMotor) - { - this(new TalonFX(id), isDriveMotor); - } - - /** - * Configure the factory defaults. - */ - @Override - public void factoryDefaults() - { - if (!factoryDefaultOccurred) - { - TalonFXConfigurator cfg = motor.getConfigurator(); - configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake; - configuration.ClosedLoopGeneral.ContinuousWrap = true; - cfg.apply(configuration); - - m_angleVoltageSetter.UpdateFreqHz = 0; -// m_angleVoltageExpoSetter.UpdateFreqHz = 0; - m_velocityVoltageSetter.UpdateFreqHz = 0; -// motor.configFactoryDefault(); -// motor.setSensorPhase(true); -// motor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30); -// motor.configNeutralDeadband(0.001); - } - } - - /** - * Clear the sticky faults on the motor controller. - */ - @Override - public void clearStickyFaults() - { - motor.clearStickyFaults(); - } - - /** - * Set the absolute encoder to be a compatible absolute encoder. - * - * @param encoder The encoder to use. - */ - @Override - public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) - { - // Do not support. - return this; - } - - /** - * Configure the integrated encoder for the swerve module. Sets the conversion factors for position and velocity. - * - * @param positionConversionFactor The conversion factor to apply for position. - *


- * Degrees:
- * - * 360 / (angleGearRatio * encoderTicksPerRotation) - *
- *


- * Meters:
- * - * (Math.PI * wheelDiameter) / (driveGearRatio * encoderTicksPerRotation) - * - */ - @Override - public void configureIntegratedEncoder(double positionConversionFactor) - { - TalonFXConfigurator cfg = motor.getConfigurator(); - cfg.refresh(configuration); - - configuration.MotionMagic = configuration.MotionMagic - .withMotionMagicCruiseVelocity(100 / positionConversionFactor) - .withMotionMagicAcceleration((100 / positionConversionFactor) / 0.100) - .withMotionMagicExpo_kV(0.12 * positionConversionFactor) - .withMotionMagicExpo_kA(5); - - configuration.Feedback = configuration.Feedback - .withSensorToMechanismRatio(positionConversionFactor) - .withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor); - - cfg.apply(configuration); - // Taken from democat's library. - // https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/Falcon500DriveControllerFactoryBuilder.java#L16 - configureCANStatusFrames(250); - } - - /** - * Set the CAN status frames. - * - * @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information - */ - public void configureCANStatusFrames(int CANStatus1) - { -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); - } - - /** - * Set the CAN status frames. - * - * @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information - * @param CANStatus2 Selected Sensor Position (PID 0), Selected Sensor Velocity (PID 0), Brushed Supply Current - * Measurement, Sticky Fault Information - * @param CANStatus3 Quadrature Information - * @param CANStatus4 Analog Input, Supply Battery Voltage, Controller Temperature - * @param CANStatus8 Pulse Width Information - * @param CANStatus10 Motion Profiling/Motion Magic Information - * @param CANStatus12 Selected Sensor Position (Aux PID 1), Selected Sensor Velocity (Aux PID 1) - * @param CANStatus13 PID0 (Primary PID) Information - * @param CANStatus14 PID1 (Auxiliary PID) Information - * @param CANStatus21 Integrated Sensor Position (Talon FX), Integrated Sensor Velocity (Talon FX) - * @param CANStatusCurrent Brushless Supply Current Measurement, Brushless Stator Current Measurement - */ - public void configureCANStatusFrames(int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4, int CANStatus8, - int CANStatus10, int CANStatus12, int CANStatus13, int CANStatus14, - int CANStatus21, int CANStatusCurrent) - { -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, CANStatus2); -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, CANStatus3); -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, CANStatus4); -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, CANStatus8); -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_Targets, CANStatus10); -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, CANStatus12); -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, CANStatus13); -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, CANStatus14); -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_21_FeedbackIntegrated, CANStatus21); -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_Brushless_Current, CANStatusCurrent); - - // TODO: Configure Status Frame 2 thru 21 if necessary - // https://v5.docs.ctr-electronics.com/en/stable/ch18_CommonAPI.html#setting-status-frame-periods - } - - /** - * Configure the PIDF values for the closed loop controller. 0 is disabled or off. - * - * @param config Configuration class holding the PIDF values. - */ - @Override - public void configurePIDF(PIDFConfig config) - { - TalonFXConfigurator cfg = motor.getConfigurator(); - cfg.refresh(configuration.Slot0); - cfg.apply(configuration.Slot0.withKP(config.p) - .withKI(config.i) - .withKD(config.d) - .withKS(config.f)); -// configuration.slot0.integralZone = config.iz; -// configuration.slot0.closedLoopPeakOutput = config.output.max; - } - - /** - * Configure the PID wrapping for the position closed loop controller. - * - * @param minInput Minimum PID input. - * @param maxInput Maximum PID input. - */ - @Override - public void configurePIDWrapping(double minInput, double maxInput) - { - TalonFXConfigurator cfg = motor.getConfigurator(); - cfg.refresh(configuration.ClosedLoopGeneral); - configuration.ClosedLoopGeneral.ContinuousWrap = true; - cfg.apply(configuration.ClosedLoopGeneral); - } - - /** - * Set the idle mode. - * - * @param isBrakeMode Set the brake mode. - */ - @Override - public void setMotorBrake(boolean isBrakeMode) - { - motor.setNeutralMode(isBrakeMode ? NeutralModeValue.Brake : NeutralModeValue.Coast); - } - - /** - * Set the motor to be inverted. - * - * @param inverted State of inversion. - */ - @Override - public void setInverted(boolean inverted) - { -// Timer.delay(1); - motor.setInverted(inverted); - } - - /** - * Save the configurations from flash to EEPROM. - */ - @Override - public void burnFlash() - { - // Do nothing - } - - /** - * Set the percentage output. - * - * @param percentOutput percent out for the motor controller. - */ - @Override - public void set(double percentOutput) - { - motor.set(percentOutput); - } - - /** - * Set the closed loop PID controller reference point. - * - * @param setpoint Setpoint in MPS or Angle in degrees. - * @param feedforward Feedforward in volt-meter-per-second or kV. - */ - @Override - public void setReference(double setpoint, double feedforward) - { - setReference(setpoint, feedforward, getPosition()); - } - - /** - * Set the closed loop PID controller reference point. - * - * @param setpoint Setpoint in meters per second or angle in degrees. - * @param feedforward Feedforward in volt-meter-per-second or kV. - * @param position Only used on the angle motor, the position of the motor in degrees. - */ - @Override - public void setReference(double setpoint, double feedforward, double position) - { -// if (SwerveDriveTelemetry.isSimulation) -// { -// PhysicsSim.getInstance().run(); -// } - - if (isDriveMotor) - { - motor.setControl(m_velocityVoltageSetter.withVelocity(setpoint)); - } else - { - // Motion magic takes input in rotations - motor.setControl(m_angleVoltageSetter.withPosition(setpoint/360)); - } - } - - /** - * Get the velocity of the integrated encoder. - * - * @return velocity in Meters Per Second, or Degrees per Second. - */ - @Override - public double getVelocity() - { - return isDriveMotor ? motor.getVelocity().getValue() : motor.getVelocity().getValue() * 360; - } - - /** - * Get the position of the integrated encoder. - * - * @return Position in Meters or Degrees. - */ - @Override - public double getPosition() - { - return isDriveMotor ? motor.getPosition().getValue() : motor.getPosition().getValue() * 360; - } - - /** - * Set the integrated encoder position. - * - * @param position Integrated encoder position. Should be angle in degrees or meters. - */ - @Override - public void setPosition(double position) - { - if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation) - { - position = position < 0 ? (position % 360) + 360 : position; - TalonFXConfigurator cfg = motor.getConfigurator(); - cfg.setPosition(position / 360); - } - } - - /** - * Set the voltage compensation for the swerve module motor. - * - * @param nominalVoltage Nominal voltage for operation to output to. - */ - @Override - public void setVoltageCompensation(double nominalVoltage) - { - // Do not implement - } - - /** - * Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with - * voltage compensation. This is useful to protect the motor from current spikes. - * - * @param currentLimit Current limit in AMPS at free speed. - */ - @Override - public void setCurrentLimit(int currentLimit) - { - TalonFXConfigurator cfg = motor.getConfigurator(); - cfg.refresh(configuration.CurrentLimits); - cfg.apply(configuration.CurrentLimits.withStatorCurrentLimit(currentLimit).withStatorCurrentLimitEnable(true)); - } - - /** - * Set the maximum rate the open/closed loop output can change by. - * - * @param rampRate Time in seconds to go from 0 to full throttle. - */ - @Override - public void setLoopRampRate(double rampRate) - { - TalonFXConfigurator cfg = motor.getConfigurator(); - cfg.refresh(configuration.ClosedLoopRamps); - cfg.apply(configuration.ClosedLoopRamps.withVoltageClosedLoopRampPeriod(rampRate)); - } - - /** - * Get the motor object from the module. - * - * @return Motor object. - */ - @Override - public Object getMotor() - { - return motor; - } - - /** - * Queries whether the absolute encoder is directly attached to the motor controller. - * - * @return connected absolute encoder state. - */ - @Override - public boolean isAttachedAbsoluteEncoder() - { - return absoluteEncoder; - } +/** {@link com.ctre.phoenix6.hardware.TalonFX} Swerve Motor. Made by Team 1466 WebbRobotics. */ +public class TalonFXSwerve extends SwerveMotor { + + /** Factory default already occurred. */ + private final boolean factoryDefaultOccurred = false; + /** Current TalonFX configuration. */ + private TalonFXConfiguration configuration = new TalonFXConfiguration(); + /** Whether the absolute encoder is integrated. */ + private final boolean absoluteEncoder = false; + /** Motion magic angle voltage setter. */ + private final MotionMagicVoltage m_angleVoltageSetter = new MotionMagicVoltage(0); + // /** + // * Motion Magic exponential voltage setters. + // */ + // private final MotionMagicExpoVoltage m_angleVoltageExpoSetter = new MotionMagicExpoVoltage(0); + /** Velocity voltage setter for controlling drive motor. */ + private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0); + /** TalonFX motor controller. */ + TalonFX motor; + + /** + * Constructor for TalonFX swerve motor. + * + * @param motor Motor to use. + * @param isDriveMotor Whether this motor is a drive motor. + */ + public TalonFXSwerve(TalonFX motor, boolean isDriveMotor) { + this.isDriveMotor = isDriveMotor; + this.motor = motor; + + factoryDefaults(); + clearStickyFaults(); + + // if (SwerveDriveTelemetry.isSimulation) + // { + //// PhysicsSim.getInstance().addTalonFX(motor, .25, 6800); + // } + } + + /** + * Construct the TalonFX swerve motor given the ID and CANBus. + * + * @param id ID of the TalonFX on the CANBus. + * @param canbus CANBus on which the TalonFX is on. + * @param isDriveMotor Whether the motor is a drive or steering motor. + */ + public TalonFXSwerve(int id, String canbus, boolean isDriveMotor) { + this(new TalonFX(id, canbus), isDriveMotor); + } + + /** + * Construct the TalonFX swerve motor given the ID. + * + * @param id ID of the TalonFX on the canbus. + * @param isDriveMotor Whether the motor is a drive or steering motor. + */ + public TalonFXSwerve(int id, boolean isDriveMotor) { + this(new TalonFX(id), isDriveMotor); + } + + /** Configure the factory defaults. */ + @Override + public void factoryDefaults() { + if (!factoryDefaultOccurred) { + TalonFXConfigurator cfg = motor.getConfigurator(); + configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake; + configuration.ClosedLoopGeneral.ContinuousWrap = true; + cfg.apply(configuration); + + m_angleVoltageSetter.UpdateFreqHz = 0; + // m_angleVoltageExpoSetter.UpdateFreqHz = 0; + m_velocityVoltageSetter.UpdateFreqHz = 0; + // motor.configFactoryDefault(); + // motor.setSensorPhase(true); + // motor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30); + // motor.configNeutralDeadband(0.001); + } + } + + /** Clear the sticky faults on the motor controller. */ + @Override + public void clearStickyFaults() { + motor.clearStickyFaults(); + } + + /** + * Set the absolute encoder to be a compatible absolute encoder. + * + * @param encoder The encoder to use. + */ + @Override + public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) { + // Do not support. + return this; + } + + /** + * Configure the integrated encoder for the swerve module. Sets the conversion factors for + * position and velocity. + * + * @param positionConversionFactor The conversion factor to apply for position. + *


+ * Degrees:
+ * + * 360 / (angleGearRatio * encoderTicksPerRotation) + *
+ *


+ * Meters:
+ * + * (Math.PI * wheelDiameter) / (driveGearRatio * encoderTicksPerRotation) + * + */ + @Override + public void configureIntegratedEncoder(double positionConversionFactor) { + TalonFXConfigurator cfg = motor.getConfigurator(); + cfg.refresh(configuration); + + configuration.MotionMagic = + configuration.MotionMagic.withMotionMagicCruiseVelocity(100 / positionConversionFactor) + .withMotionMagicAcceleration((100 / positionConversionFactor) / 0.100) + .withMotionMagicExpo_kV(0.12 * positionConversionFactor) + .withMotionMagicExpo_kA(5); + + configuration.Feedback = + configuration.Feedback.withSensorToMechanismRatio(positionConversionFactor) + .withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor); + + cfg.apply(configuration); + // Taken from democat's library. + // https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/Falcon500DriveControllerFactoryBuilder.java#L16 + configureCANStatusFrames(250); + } + + /** + * Set the CAN status frames. + * + * @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information + */ + public void configureCANStatusFrames(int CANStatus1) { + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); + } + + /** + * Set the CAN status frames. + * + * @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information + * @param CANStatus2 Selected Sensor Position (PID 0), Selected Sensor Velocity (PID 0), Brushed + * Supply Current Measurement, Sticky Fault Information + * @param CANStatus3 Quadrature Information + * @param CANStatus4 Analog Input, Supply Battery Voltage, Controller Temperature + * @param CANStatus8 Pulse Width Information + * @param CANStatus10 Motion Profiling/Motion Magic Information + * @param CANStatus12 Selected Sensor Position (Aux PID 1), Selected Sensor Velocity (Aux PID 1) + * @param CANStatus13 PID0 (Primary PID) Information + * @param CANStatus14 PID1 (Auxiliary PID) Information + * @param CANStatus21 Integrated Sensor Position (Talon FX), Integrated Sensor Velocity (Talon FX) + * @param CANStatusCurrent Brushless Supply Current Measurement, Brushless Stator Current + * Measurement + */ + public void configureCANStatusFrames( + int CANStatus1, + int CANStatus2, + int CANStatus3, + int CANStatus4, + int CANStatus8, + int CANStatus10, + int CANStatus12, + int CANStatus13, + int CANStatus14, + int CANStatus21, + int CANStatusCurrent) { + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, CANStatus2); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, CANStatus3); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, CANStatus4); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, CANStatus8); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_Targets, CANStatus10); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, CANStatus12); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, CANStatus13); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, CANStatus14); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_21_FeedbackIntegrated, CANStatus21); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_Brushless_Current, + // CANStatusCurrent); + + // TODO: Configure Status Frame 2 thru 21 if necessary + // https://v5.docs.ctr-electronics.com/en/stable/ch18_CommonAPI.html#setting-status-frame-periods + } + + /** + * Configure the PIDF values for the closed loop controller. 0 is disabled or off. + * + * @param config Configuration class holding the PIDF values. + */ + @Override + public void configurePIDF(PIDFConfig config) { + TalonFXConfigurator cfg = motor.getConfigurator(); + cfg.refresh(configuration.Slot0); + cfg.apply( + configuration.Slot0.withKP(config.p).withKI(config.i).withKD(config.d).withKS(config.f)); + // configuration.slot0.integralZone = config.iz; + // configuration.slot0.closedLoopPeakOutput = config.output.max; + } + + /** + * Configure the PID wrapping for the position closed loop controller. + * + * @param minInput Minimum PID input. + * @param maxInput Maximum PID input. + */ + @Override + public void configurePIDWrapping(double minInput, double maxInput) { + TalonFXConfigurator cfg = motor.getConfigurator(); + cfg.refresh(configuration.ClosedLoopGeneral); + configuration.ClosedLoopGeneral.ContinuousWrap = true; + cfg.apply(configuration.ClosedLoopGeneral); + } + + /** + * Set the idle mode. + * + * @param isBrakeMode Set the brake mode. + */ + @Override + public void setMotorBrake(boolean isBrakeMode) { + motor.setNeutralMode(isBrakeMode ? NeutralModeValue.Brake : NeutralModeValue.Coast); + } + + /** + * Set the motor to be inverted. + * + * @param inverted State of inversion. + */ + @Override + public void setInverted(boolean inverted) { + // Timer.delay(1); + motor.setInverted(inverted); + } + + /** Save the configurations from flash to EEPROM. */ + @Override + public void burnFlash() { + // Do nothing + } + + /** + * Set the percentage output. + * + * @param percentOutput percent out for the motor controller. + */ + @Override + public void set(double percentOutput) { + motor.set(percentOutput); + } + + /** + * Set the closed loop PID controller reference point. + * + * @param setpoint Setpoint in MPS or Angle in degrees. + * @param feedforward Feedforward in volt-meter-per-second or kV. + */ + @Override + public void setReference(double setpoint, double feedforward) { + setReference(setpoint, feedforward, getPosition()); + } + + /** + * Set the closed loop PID controller reference point. + * + * @param setpoint Setpoint in meters per second or angle in degrees. + * @param feedforward Feedforward in volt-meter-per-second or kV. + * @param position Only used on the angle motor, the position of the motor in degrees. + */ + @Override + public void setReference(double setpoint, double feedforward, double position) { + // if (SwerveDriveTelemetry.isSimulation) + // { + // PhysicsSim.getInstance().run(); + // } + + if (isDriveMotor) { + motor.setControl(m_velocityVoltageSetter.withVelocity(setpoint)); + } else { + // Motion magic takes input in rotations + motor.setControl(m_angleVoltageSetter.withPosition(setpoint / 360)); + } + } + + /** + * Get the velocity of the integrated encoder. + * + * @return velocity in Meters Per Second, or Degrees per Second. + */ + @Override + public double getVelocity() { + return isDriveMotor ? motor.getVelocity().getValue() : motor.getVelocity().getValue() * 360; + } + + /** + * Get the position of the integrated encoder. + * + * @return Position in Meters or Degrees. + */ + @Override + public double getPosition() { + return isDriveMotor ? motor.getPosition().getValue() : motor.getPosition().getValue() * 360; + } + + /** + * Set the integrated encoder position. + * + * @param position Integrated encoder position. Should be angle in degrees or meters. + */ + @Override + public void setPosition(double position) { + if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation) { + position = position < 0 ? (position % 360) + 360 : position; + TalonFXConfigurator cfg = motor.getConfigurator(); + cfg.setPosition(position / 360); + } + } + + /** + * Set the voltage compensation for the swerve module motor. + * + * @param nominalVoltage Nominal voltage for operation to output to. + */ + @Override + public void setVoltageCompensation(double nominalVoltage) { + // Do not implement + } + + /** + * Set the current limit for the swerve drive motor, remember this may cause jumping if used in + * conjunction with voltage compensation. This is useful to protect the motor from current spikes. + * + * @param currentLimit Current limit in AMPS at free speed. + */ + @Override + public void setCurrentLimit(int currentLimit) { + TalonFXConfigurator cfg = motor.getConfigurator(); + cfg.refresh(configuration.CurrentLimits); + cfg.apply( + configuration.CurrentLimits.withStatorCurrentLimit(currentLimit) + .withStatorCurrentLimitEnable(true)); + } + + /** + * Set the maximum rate the open/closed loop output can change by. + * + * @param rampRate Time in seconds to go from 0 to full throttle. + */ + @Override + public void setLoopRampRate(double rampRate) { + TalonFXConfigurator cfg = motor.getConfigurator(); + cfg.refresh(configuration.ClosedLoopRamps); + cfg.apply(configuration.ClosedLoopRamps.withVoltageClosedLoopRampPeriod(rampRate)); + } + + /** + * Get the motor object from the module. + * + * @return Motor object. + */ + @Override + public Object getMotor() { + return motor; + } + + /** + * Queries whether the absolute encoder is directly attached to the motor controller. + * + * @return connected absolute encoder state. + */ + @Override + public boolean isAttachedAbsoluteEncoder() { + return absoluteEncoder; + } } diff --git a/src/main/java/swervelib/motors/TalonSRXSwerve.java b/src/main/java/swervelib/motors/TalonSRXSwerve.java index 0290d828..403d33df 100644 --- a/src/main/java/swervelib/motors/TalonSRXSwerve.java +++ b/src/main/java/swervelib/motors/TalonSRXSwerve.java @@ -13,403 +13,361 @@ import swervelib.parser.PIDFConfig; import swervelib.telemetry.SwerveDriveTelemetry; -/** - * {@link com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX} Swerve Motor. - */ -public class TalonSRXSwerve extends SwerveMotor -{ - - /** - * Factory default already occurred. - */ - private final boolean factoryDefaultOccurred = false; - /** - * Current TalonFX configuration. - */ - private final TalonSRXConfiguration configuration = new TalonSRXConfiguration(); - /** - * Whether the absolute encoder is integrated. - */ - private final boolean absoluteEncoder = false; - /** - * TalonSRX motor controller. - */ - WPI_TalonSRX motor; - /** - * The position conversion factor to convert raw sensor units to Meters Per 100ms, or Ticks to Degrees. - */ - private double positionConversionFactor = 1; - /** - * If the TalonFX configuration has changed. - */ - private boolean configChanged = true; - /** - * Nominal voltage default to use with feedforward. - */ - private double nominalVoltage = 12.0; - - /** - * Constructor for TalonSRX swerve motor. - * - * @param motor Motor to use. - * @param isDriveMotor Whether this motor is a drive motor. - */ - public TalonSRXSwerve(WPI_TalonSRX motor, boolean isDriveMotor) - { - this.isDriveMotor = isDriveMotor; - this.motor = motor; - motor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); - - factoryDefaults(); - clearStickyFaults(); - - } - - /** - * Construct the TalonSRX swerve motor given the ID. - * - * @param id ID of the TalonSRX on the canbus. - * @param isDriveMotor Whether the motor is a drive or steering motor. - */ - public TalonSRXSwerve(int id, boolean isDriveMotor) - { - this(new WPI_TalonSRX(id), isDriveMotor); - } - - /** - * Configure the factory defaults. - */ - @Override - public void factoryDefaults() - { - if (!factoryDefaultOccurred) - { - motor.configFactoryDefault(); - motor.setSensorPhase(true); - } - } - - /** - * Clear the sticky faults on the motor controller. - */ - @Override - public void clearStickyFaults() - { - motor.clearStickyFaults(); - } - - /** - * Set the absolute encoder to be a compatible absolute encoder. - * - * @param encoder The encoder to use. - */ - @Override - public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) - { - // Do not support. - return this; - } - - /** - * Configure the integrated encoder for the swerve module. Sets the conversion factors for position and velocity. - * - * @param positionConversionFactor The conversion factor to apply for position. - *


- * Degrees:
- * - * 360 / (angleGearRatio * encoderTicksPerRotation) - *
- *


- * Meters:
- * - * (Math.PI * wheelDiameter) / (driveGearRatio * encoderTicksPerRotation) - * - */ - @Override - public void configureIntegratedEncoder(double positionConversionFactor) - { - this.positionConversionFactor = positionConversionFactor; - // Taken from democat's library. - // https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/Falcon500DriveControllerFactoryBuilder.java#L16 - configureCANStatusFrames(250); - } - - /** - * Set the CAN status frames. - * - * @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information - */ - public void configureCANStatusFrames(int CANStatus1) - { - motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); - } - - /** - * Set the CAN status frames. - * - * @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information - * @param CANStatus2 Selected Sensor Position (PID 0), Selected Sensor Velocity (PID 0), Brushed Supply Current - * Measurement, Sticky Fault Information - * @param CANStatus3 Quadrature Information - * @param CANStatus4 Analog Input, Supply Battery Voltage, Controller Temperature - * @param CANStatus8 Pulse Width Information - * @param CANStatus10 Motion Profiling/Motion Magic Information - * @param CANStatus12 Selected Sensor Position (Aux PID 1), Selected Sensor Velocity (Aux PID 1) - * @param CANStatus13 PID0 (Primary PID) Information - * @param CANStatus14 PID1 (Auxiliary PID) Information - * @param CANStatus21 Integrated Sensor Position (Talon FX), Integrated Sensor Velocity (Talon FX) - * @param CANStatusCurrent Brushless Supply Current Measurement, Brushless Stator Current Measurement - */ - public void configureCANStatusFrames(int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4, int CANStatus8, - int CANStatus10, int CANStatus12, int CANStatus13, int CANStatus14, - int CANStatus21, int CANStatusCurrent) - { - motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); - motor.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, CANStatus2); - motor.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, CANStatus3); - motor.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, CANStatus4); - motor.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, CANStatus8); - motor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_Targets, CANStatus10); - motor.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, CANStatus12); - motor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, CANStatus13); - motor.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, CANStatus14); - motor.setStatusFramePeriod(StatusFrameEnhanced.Status_21_FeedbackIntegrated, CANStatus21); - motor.setStatusFramePeriod(StatusFrameEnhanced.Status_Brushless_Current, CANStatusCurrent); - - // TODO: Configure Status Frame 2 thru 21 if necessary - // https://v5.docs.ctr-electronics.com/en/stable/ch18_CommonAPI.html#setting-status-frame-periods - } - - /** - * Configure the PIDF values for the closed loop controller. 0 is disabled or off. - * - * @param config Configuration class holding the PIDF values. - */ - @Override - public void configurePIDF(PIDFConfig config) - { - configuration.slot0.kP = config.p; - configuration.slot0.kI = config.i; - configuration.slot0.kD = config.d; - configuration.slot0.kF = config.f; - configuration.slot0.integralZone = config.iz; - configuration.slot0.closedLoopPeakOutput = config.output.max; - configChanged = true; - } - - /** - * Configure the PID wrapping for the position closed loop controller. - * - * @param minInput Minimum PID input. - * @param maxInput Maximum PID input. - */ - @Override - public void configurePIDWrapping(double minInput, double maxInput) - { - // Do nothing - } - - /** - * Set the idle mode. - * - * @param isBrakeMode Set the brake mode. - */ - @Override - public void setMotorBrake(boolean isBrakeMode) - { - motor.setNeutralMode(isBrakeMode ? NeutralMode.Brake : NeutralMode.Coast); - } - - /** - * Set the motor to be inverted. - * - * @param inverted State of inversion. - */ - @Override - public void setInverted(boolean inverted) - { - Timer.delay(1); - motor.setInverted(inverted); - } - - /** - * Save the configurations from flash to EEPROM. - */ - @Override - public void burnFlash() - { - if (configChanged) - { - motor.configAllSettings(configuration, 250); - configChanged = false; - } - } - - /** - * Set the percentage output. - * - * @param percentOutput percent out for the motor controller. - */ - @Override - public void set(double percentOutput) - { - motor.set(percentOutput); - } - - - /** - * Convert the setpoint into native sensor units. - * - * @param setpoint Setpoint to mutate. In meters per second or degrees. - * @param position Position in degrees, only used on angle motors. - * @return Setpoint as native sensor units. Encoder ticks per 100ms, or Encoder tick. - */ - public double convertToNativeSensorUnits(double setpoint, double position) - { - setpoint = - isDriveMotor ? setpoint * .1 : SwerveMath.placeInAppropriate0To360Scope(position, setpoint); - return setpoint / positionConversionFactor; - } - - /** - * Set the closed loop PID controller reference point. - * - * @param setpoint Setpoint in MPS or Angle in degrees. - * @param feedforward Feedforward in volt-meter-per-second or kV. - */ - @Override - public void setReference(double setpoint, double feedforward) - { - setReference(setpoint, feedforward, getPosition()); - } - - /** - * Set the closed loop PID controller reference point. - * - * @param setpoint Setpoint in meters per second or angle in degrees. - * @param feedforward Feedforward in volt-meter-per-second or kV. - * @param position Only used on the angle motor, the position of the motor in degrees. - */ - @Override - public void setReference(double setpoint, double feedforward, double position) - { - - burnFlash(); - - motor.set( - isDriveMotor ? ControlMode.Velocity : ControlMode.Position, - convertToNativeSensorUnits(setpoint, position), - DemandType.ArbitraryFeedForward, - feedforward / nominalVoltage); - } - - /** - * Get the velocity of the integrated encoder. - * - * @return velocity in Meters Per Second, or Degrees per Second. - */ - @Override - public double getVelocity() - { - return (motor.getSelectedSensorVelocity() * 10) * positionConversionFactor; - } - - /** - * Get the position of the integrated encoder. - * - * @return Position in Meters or Degrees. - */ - @Override - public double getPosition() - { - if (isDriveMotor) - { - return motor.getSelectedSensorPosition() * positionConversionFactor; - } else - { - var pos = motor.getSelectedSensorPosition() * positionConversionFactor; - pos %= 360; - if (pos < 360) - { - pos += 360; - } - return pos; - } - } - - /** - * Set the integrated encoder position. - * - * @param position Integrated encoder position. Should be angle in degrees or meters. - */ - @Override - public void setPosition(double position) - { - if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation) - { - motor.setSelectedSensorPosition(position / positionConversionFactor, 0, 0); - } - } - - /** - * Set the voltage compensation for the swerve module motor. - * - * @param nominalVoltage Nominal voltage for operation to output to. - */ - @Override - public void setVoltageCompensation(double nominalVoltage) - { - configuration.voltageCompSaturation = nominalVoltage; - configChanged = true; - this.nominalVoltage = nominalVoltage; - } - - /** - * Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with - * voltage compensation. This is useful to protect the motor from current spikes. - * - * @param currentLimit Current limit in AMPS at free speed. - */ - @Override - public void setCurrentLimit(int currentLimit) - { - configuration.continuousCurrentLimit = currentLimit; - configuration.peakCurrentLimit = currentLimit; - configChanged = true; - } - - /** - * Set the maximum rate the open/closed loop output can change by. - * - * @param rampRate Time in seconds to go from 0 to full throttle. - */ - @Override - public void setLoopRampRate(double rampRate) - { - configuration.closedloopRamp = rampRate; - configuration.openloopRamp = rampRate; - configChanged = true; - } - - /** - * Get the motor object from the module. - * - * @return Motor object. - */ - @Override - public Object getMotor() - { - return motor; - } - - /** - * Queries whether the absolute encoder is directly attached to the motor controller. - * - * @return connected absolute encoder state. - */ - @Override - public boolean isAttachedAbsoluteEncoder() - { - return absoluteEncoder; - } +/** {@link com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX} Swerve Motor. */ +public class TalonSRXSwerve extends SwerveMotor { + + /** Factory default already occurred. */ + private final boolean factoryDefaultOccurred = false; + /** Current TalonFX configuration. */ + private final TalonSRXConfiguration configuration = new TalonSRXConfiguration(); + /** Whether the absolute encoder is integrated. */ + private final boolean absoluteEncoder = false; + /** TalonSRX motor controller. */ + WPI_TalonSRX motor; + /** + * The position conversion factor to convert raw sensor units to Meters Per 100ms, or Ticks to + * Degrees. + */ + private double positionConversionFactor = 1; + /** If the TalonFX configuration has changed. */ + private boolean configChanged = true; + /** Nominal voltage default to use with feedforward. */ + private double nominalVoltage = 12.0; + + /** + * Constructor for TalonSRX swerve motor. + * + * @param motor Motor to use. + * @param isDriveMotor Whether this motor is a drive motor. + */ + public TalonSRXSwerve(WPI_TalonSRX motor, boolean isDriveMotor) { + this.isDriveMotor = isDriveMotor; + this.motor = motor; + motor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); + + factoryDefaults(); + clearStickyFaults(); + } + + /** + * Construct the TalonSRX swerve motor given the ID. + * + * @param id ID of the TalonSRX on the canbus. + * @param isDriveMotor Whether the motor is a drive or steering motor. + */ + public TalonSRXSwerve(int id, boolean isDriveMotor) { + this(new WPI_TalonSRX(id), isDriveMotor); + } + + /** Configure the factory defaults. */ + @Override + public void factoryDefaults() { + if (!factoryDefaultOccurred) { + motor.configFactoryDefault(); + motor.setSensorPhase(true); + } + } + + /** Clear the sticky faults on the motor controller. */ + @Override + public void clearStickyFaults() { + motor.clearStickyFaults(); + } + + /** + * Set the absolute encoder to be a compatible absolute encoder. + * + * @param encoder The encoder to use. + */ + @Override + public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) { + // Do not support. + return this; + } + + /** + * Configure the integrated encoder for the swerve module. Sets the conversion factors for + * position and velocity. + * + * @param positionConversionFactor The conversion factor to apply for position. + *


+ * Degrees:
+ * + * 360 / (angleGearRatio * encoderTicksPerRotation) + *
+ *


+ * Meters:
+ * + * (Math.PI * wheelDiameter) / (driveGearRatio * encoderTicksPerRotation) + * + */ + @Override + public void configureIntegratedEncoder(double positionConversionFactor) { + this.positionConversionFactor = positionConversionFactor; + // Taken from democat's library. + // https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/Falcon500DriveControllerFactoryBuilder.java#L16 + configureCANStatusFrames(250); + } + + /** + * Set the CAN status frames. + * + * @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information + */ + public void configureCANStatusFrames(int CANStatus1) { + motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); + } + + /** + * Set the CAN status frames. + * + * @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information + * @param CANStatus2 Selected Sensor Position (PID 0), Selected Sensor Velocity (PID 0), Brushed + * Supply Current Measurement, Sticky Fault Information + * @param CANStatus3 Quadrature Information + * @param CANStatus4 Analog Input, Supply Battery Voltage, Controller Temperature + * @param CANStatus8 Pulse Width Information + * @param CANStatus10 Motion Profiling/Motion Magic Information + * @param CANStatus12 Selected Sensor Position (Aux PID 1), Selected Sensor Velocity (Aux PID 1) + * @param CANStatus13 PID0 (Primary PID) Information + * @param CANStatus14 PID1 (Auxiliary PID) Information + * @param CANStatus21 Integrated Sensor Position (Talon FX), Integrated Sensor Velocity (Talon FX) + * @param CANStatusCurrent Brushless Supply Current Measurement, Brushless Stator Current + * Measurement + */ + public void configureCANStatusFrames( + int CANStatus1, + int CANStatus2, + int CANStatus3, + int CANStatus4, + int CANStatus8, + int CANStatus10, + int CANStatus12, + int CANStatus13, + int CANStatus14, + int CANStatus21, + int CANStatusCurrent) { + motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); + motor.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, CANStatus2); + motor.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, CANStatus3); + motor.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, CANStatus4); + motor.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, CANStatus8); + motor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_Targets, CANStatus10); + motor.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, CANStatus12); + motor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, CANStatus13); + motor.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, CANStatus14); + motor.setStatusFramePeriod(StatusFrameEnhanced.Status_21_FeedbackIntegrated, CANStatus21); + motor.setStatusFramePeriod(StatusFrameEnhanced.Status_Brushless_Current, CANStatusCurrent); + + // TODO: Configure Status Frame 2 thru 21 if necessary + // https://v5.docs.ctr-electronics.com/en/stable/ch18_CommonAPI.html#setting-status-frame-periods + } + + /** + * Configure the PIDF values for the closed loop controller. 0 is disabled or off. + * + * @param config Configuration class holding the PIDF values. + */ + @Override + public void configurePIDF(PIDFConfig config) { + configuration.slot0.kP = config.p; + configuration.slot0.kI = config.i; + configuration.slot0.kD = config.d; + configuration.slot0.kF = config.f; + configuration.slot0.integralZone = config.iz; + configuration.slot0.closedLoopPeakOutput = config.output.max; + configChanged = true; + } + + /** + * Configure the PID wrapping for the position closed loop controller. + * + * @param minInput Minimum PID input. + * @param maxInput Maximum PID input. + */ + @Override + public void configurePIDWrapping(double minInput, double maxInput) { + // Do nothing + } + + /** + * Set the idle mode. + * + * @param isBrakeMode Set the brake mode. + */ + @Override + public void setMotorBrake(boolean isBrakeMode) { + motor.setNeutralMode(isBrakeMode ? NeutralMode.Brake : NeutralMode.Coast); + } + + /** + * Set the motor to be inverted. + * + * @param inverted State of inversion. + */ + @Override + public void setInverted(boolean inverted) { + Timer.delay(1); + motor.setInverted(inverted); + } + + /** Save the configurations from flash to EEPROM. */ + @Override + public void burnFlash() { + if (configChanged) { + motor.configAllSettings(configuration, 250); + configChanged = false; + } + } + + /** + * Set the percentage output. + * + * @param percentOutput percent out for the motor controller. + */ + @Override + public void set(double percentOutput) { + motor.set(percentOutput); + } + + /** + * Convert the setpoint into native sensor units. + * + * @param setpoint Setpoint to mutate. In meters per second or degrees. + * @param position Position in degrees, only used on angle motors. + * @return Setpoint as native sensor units. Encoder ticks per 100ms, or Encoder tick. + */ + public double convertToNativeSensorUnits(double setpoint, double position) { + setpoint = + isDriveMotor ? setpoint * .1 : SwerveMath.placeInAppropriate0To360Scope(position, setpoint); + return setpoint / positionConversionFactor; + } + + /** + * Set the closed loop PID controller reference point. + * + * @param setpoint Setpoint in MPS or Angle in degrees. + * @param feedforward Feedforward in volt-meter-per-second or kV. + */ + @Override + public void setReference(double setpoint, double feedforward) { + setReference(setpoint, feedforward, getPosition()); + } + + /** + * Set the closed loop PID controller reference point. + * + * @param setpoint Setpoint in meters per second or angle in degrees. + * @param feedforward Feedforward in volt-meter-per-second or kV. + * @param position Only used on the angle motor, the position of the motor in degrees. + */ + @Override + public void setReference(double setpoint, double feedforward, double position) { + + burnFlash(); + + motor.set( + isDriveMotor ? ControlMode.Velocity : ControlMode.Position, + convertToNativeSensorUnits(setpoint, position), + DemandType.ArbitraryFeedForward, + feedforward / nominalVoltage); + } + + /** + * Get the velocity of the integrated encoder. + * + * @return velocity in Meters Per Second, or Degrees per Second. + */ + @Override + public double getVelocity() { + return (motor.getSelectedSensorVelocity() * 10) * positionConversionFactor; + } + + /** + * Get the position of the integrated encoder. + * + * @return Position in Meters or Degrees. + */ + @Override + public double getPosition() { + if (isDriveMotor) { + return motor.getSelectedSensorPosition() * positionConversionFactor; + } else { + var pos = motor.getSelectedSensorPosition() * positionConversionFactor; + pos %= 360; + if (pos < 360) { + pos += 360; + } + return pos; + } + } + + /** + * Set the integrated encoder position. + * + * @param position Integrated encoder position. Should be angle in degrees or meters. + */ + @Override + public void setPosition(double position) { + if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation) { + motor.setSelectedSensorPosition(position / positionConversionFactor, 0, 0); + } + } + + /** + * Set the voltage compensation for the swerve module motor. + * + * @param nominalVoltage Nominal voltage for operation to output to. + */ + @Override + public void setVoltageCompensation(double nominalVoltage) { + configuration.voltageCompSaturation = nominalVoltage; + configChanged = true; + this.nominalVoltage = nominalVoltage; + } + + /** + * Set the current limit for the swerve drive motor, remember this may cause jumping if used in + * conjunction with voltage compensation. This is useful to protect the motor from current spikes. + * + * @param currentLimit Current limit in AMPS at free speed. + */ + @Override + public void setCurrentLimit(int currentLimit) { + configuration.continuousCurrentLimit = currentLimit; + configuration.peakCurrentLimit = currentLimit; + configChanged = true; + } + + /** + * Set the maximum rate the open/closed loop output can change by. + * + * @param rampRate Time in seconds to go from 0 to full throttle. + */ + @Override + public void setLoopRampRate(double rampRate) { + configuration.closedloopRamp = rampRate; + configuration.openloopRamp = rampRate; + configChanged = true; + } + + /** + * Get the motor object from the module. + * + * @return Motor object. + */ + @Override + public Object getMotor() { + return motor; + } + + /** + * Queries whether the absolute encoder is directly attached to the motor controller. + * + * @return connected absolute encoder state. + */ + @Override + public boolean isAttachedAbsoluteEncoder() { + return absoluteEncoder; + } } diff --git a/src/main/java/swervelib/motors/package-info.java b/src/main/java/swervelib/motors/package-info.java index d8bd36c2..de65fb08 100644 --- a/src/main/java/swervelib/motors/package-info.java +++ b/src/main/java/swervelib/motors/package-info.java @@ -1,4 +1,2 @@ -/** - * Swerve motor controller wrappers which implement {@link swervelib.motors.SwerveMotor}. - */ +/** Swerve motor controller wrappers which implement {@link swervelib.motors.SwerveMotor}. */ package swervelib.motors; diff --git a/src/main/java/swervelib/parser/PIDFConfig.java b/src/main/java/swervelib/parser/PIDFConfig.java index 9da06849..b6451aa6 100644 --- a/src/main/java/swervelib/parser/PIDFConfig.java +++ b/src/main/java/swervelib/parser/PIDFConfig.java @@ -3,106 +3,82 @@ import edu.wpi.first.math.controller.PIDController; import swervelib.parser.deserializer.PIDFRange; -/** - * Hold the PIDF and Integral Zone values for a PID. - */ -public class PIDFConfig -{ +/** Hold the PIDF and Integral Zone values for a PID. */ +public class PIDFConfig { - /** - * Proportional Gain for PID. - */ - public double p; - /** - * Integral Gain for PID. - */ - public double i; - /** - * Derivative Gain for PID. - */ - public double d; - /** - * Feedforward value for PID. - */ - public double f; - /** - * Integral zone of the PID. - */ - public double iz; + /** Proportional Gain for PID. */ + public double p; + /** Integral Gain for PID. */ + public double i; + /** Derivative Gain for PID. */ + public double d; + /** Feedforward value for PID. */ + public double f; + /** Integral zone of the PID. */ + public double iz; - /** - * The PIDF output range. - */ - public PIDFRange output = new PIDFRange(); + /** The PIDF output range. */ + public PIDFRange output = new PIDFRange(); - /** - * Used when parsing PIDF values from JSON. - */ - public PIDFConfig() - { - } + /** Used when parsing PIDF values from JSON. */ + public PIDFConfig() {} - /** - * PIDF Config constructor to contain the values. - * - * @param p P gain. - * @param i I gain. - * @param d D gain. - * @param f F gain. - * @param iz Intergral zone. - */ - public PIDFConfig(double p, double i, double d, double f, double iz) - { - this.p = p; - this.i = i; - this.d = d; - this.f = f; - this.iz = iz; - } + /** + * PIDF Config constructor to contain the values. + * + * @param p P gain. + * @param i I gain. + * @param d D gain. + * @param f F gain. + * @param iz Intergral zone. + */ + public PIDFConfig(double p, double i, double d, double f, double iz) { + this.p = p; + this.i = i; + this.d = d; + this.f = f; + this.iz = iz; + } - /** - * PIDF Config constructor to contain the values. - * - * @param p P gain. - * @param i I gain. - * @param d D gain. - * @param f F gain. - */ - public PIDFConfig(double p, double i, double d, double f) - { - this(p, i, d, f, 0); - } + /** + * PIDF Config constructor to contain the values. + * + * @param p P gain. + * @param i I gain. + * @param d D gain. + * @param f F gain. + */ + public PIDFConfig(double p, double i, double d, double f) { + this(p, i, d, f, 0); + } - /** - * PIDF Config constructor to contain the values. - * - * @param p P gain. - * @param i I gain. - * @param d D gain. - */ - public PIDFConfig(double p, double i, double d) - { - this(p, i, d, 0, 0); - } + /** + * PIDF Config constructor to contain the values. + * + * @param p P gain. + * @param i I gain. + * @param d D gain. + */ + public PIDFConfig(double p, double i, double d) { + this(p, i, d, 0, 0); + } - /** - * PIDF Config constructor to contain the values. - * - * @param p P gain. - * @param d D gain. - */ - public PIDFConfig(double p, double d) - { - this(p, 0, d, 0, 0); - } + /** + * PIDF Config constructor to contain the values. + * + * @param p P gain. + * @param d D gain. + */ + public PIDFConfig(double p, double d) { + this(p, 0, d, 0, 0); + } - /** - * Create a PIDController from the PID values. - * - * @return PIDController. - */ - public PIDController createPIDController() - { - return new PIDController(p, i, d); - } + /** + * Create a PIDController from the PID values. + * + * @return PIDController. + */ + public PIDController createPIDController() { + return new PIDController(p, i, d); + } } diff --git a/src/main/java/swervelib/parser/SwerveControllerConfiguration.java b/src/main/java/swervelib/parser/SwerveControllerConfiguration.java index 7f1d9671..7ace09d3 100644 --- a/src/main/java/swervelib/parser/SwerveControllerConfiguration.java +++ b/src/main/java/swervelib/parser/SwerveControllerConfiguration.java @@ -3,61 +3,55 @@ import static swervelib.math.SwerveMath.calculateMaxAngularVelocity; /** - * Swerve Controller configuration class which is used to configure {@link swervelib.SwerveController}. + * Swerve Controller configuration class which is used to configure {@link + * swervelib.SwerveController}. */ -public class SwerveControllerConfiguration -{ +public class SwerveControllerConfiguration { - /** - * PIDF for the heading of the robot. - */ - public final PIDFConfig headingPIDF; - /** - * hypotenuse deadband for the robot angle control joystick. - */ - public final double - angleJoyStickRadiusDeadband; // Deadband for the minimum hypot for the heading joystick. - /** - * Maximum angular velocity in rad/s - */ - public double maxAngularVelocity; + /** PIDF for the heading of the robot. */ + public final PIDFConfig headingPIDF; + /** hypotenuse deadband for the robot angle control joystick. */ + public final double + angleJoyStickRadiusDeadband; // Deadband for the minimum hypot for the heading joystick. + /** Maximum angular velocity in rad/s */ + public double maxAngularVelocity; - /** - * Construct the swerve controller configuration. Assumes robot is square to fetch maximum angular velocity. - * - * @param driveCfg {@link SwerveDriveConfiguration} to fetch the first module X and Y used to - * calculate the maximum angular velocity. - * @param headingPIDF Heading PIDF configuration. - * @param angleJoyStickRadiusDeadband Deadband on radius of angle joystick. - * @param maxSpeedMPS Maximum speed in meters per second for angular velocity, remember if you have - * feet per second use {@link edu.wpi.first.math.util.Units#feetToMeters(double)}. - */ - public SwerveControllerConfiguration( - SwerveDriveConfiguration driveCfg, - PIDFConfig headingPIDF, - double angleJoyStickRadiusDeadband, - double maxSpeedMPS) - { - this.maxAngularVelocity = - calculateMaxAngularVelocity( - maxSpeedMPS, - Math.abs(driveCfg.moduleLocationsMeters[0].getX()), - Math.abs(driveCfg.moduleLocationsMeters[0].getY())); - this.headingPIDF = headingPIDF; - this.angleJoyStickRadiusDeadband = angleJoyStickRadiusDeadband; - } + /** + * Construct the swerve controller configuration. Assumes robot is square to fetch maximum angular + * velocity. + * + * @param driveCfg {@link SwerveDriveConfiguration} to fetch the first module X and Y used to + * calculate the maximum angular velocity. + * @param headingPIDF Heading PIDF configuration. + * @param angleJoyStickRadiusDeadband Deadband on radius of angle joystick. + * @param maxSpeedMPS Maximum speed in meters per second for angular velocity, remember if you + * have feet per second use {@link edu.wpi.first.math.util.Units#feetToMeters(double)}. + */ + public SwerveControllerConfiguration( + SwerveDriveConfiguration driveCfg, + PIDFConfig headingPIDF, + double angleJoyStickRadiusDeadband, + double maxSpeedMPS) { + this.maxAngularVelocity = + calculateMaxAngularVelocity( + maxSpeedMPS, + Math.abs(driveCfg.moduleLocationsMeters[0].getX()), + Math.abs(driveCfg.moduleLocationsMeters[0].getY())); + this.headingPIDF = headingPIDF; + this.angleJoyStickRadiusDeadband = angleJoyStickRadiusDeadband; + } - /** - * Construct the swerve controller configuration. Assumes hypotenuse deadband of 0.5 (minimum radius for angle to be - * set on angle joystick is .5 of the controller). - * - * @param driveCfg Drive configuration. - * @param headingPIDF Heading PIDF configuration. - * @param maxSpeedMPS Maximum speed in meters per second for angular velocity, remember if you have feet per second - * use {@link edu.wpi.first.math.util.Units#feetToMeters(double)}. - */ - public SwerveControllerConfiguration(SwerveDriveConfiguration driveCfg, PIDFConfig headingPIDF, double maxSpeedMPS) - { - this(driveCfg, headingPIDF, 0.5, maxSpeedMPS); - } + /** + * Construct the swerve controller configuration. Assumes hypotenuse deadband of 0.5 (minimum + * radius for angle to be set on angle joystick is .5 of the controller). + * + * @param driveCfg Drive configuration. + * @param headingPIDF Heading PIDF configuration. + * @param maxSpeedMPS Maximum speed in meters per second for angular velocity, remember if you + * have feet per second use {@link edu.wpi.first.math.util.Units#feetToMeters(double)}. + */ + public SwerveControllerConfiguration( + SwerveDriveConfiguration driveCfg, PIDFConfig headingPIDF, double maxSpeedMPS) { + this(driveCfg, headingPIDF, 0.5, maxSpeedMPS); + } } diff --git a/src/main/java/swervelib/parser/SwerveDriveConfiguration.java b/src/main/java/swervelib/parser/SwerveDriveConfiguration.java index f0df58ac..16c085ab 100644 --- a/src/main/java/swervelib/parser/SwerveDriveConfiguration.java +++ b/src/main/java/swervelib/parser/SwerveDriveConfiguration.java @@ -5,91 +5,74 @@ import swervelib.SwerveModule; import swervelib.imu.SwerveIMU; -/** - * Swerve drive configurations used during SwerveDrive construction. - */ -public class SwerveDriveConfiguration -{ +/** Swerve drive configurations used during SwerveDrive construction. */ +public class SwerveDriveConfiguration { - /** - * Swerve Module locations. - */ - public Translation2d[] moduleLocationsMeters; - /** - * Swerve IMU - */ - public SwerveIMU imu; - /** - * Invert the imu measurements. - */ - public boolean invertedIMU = false; - /** - * Number of modules on the robot. - */ - public int moduleCount; - /** - * Swerve Modules. - */ - public SwerveModule[] modules; - /** - * Physical characteristics of the swerve drive from physicalproperties.json. - */ - public SwerveModulePhysicalCharacteristics physicalCharacteristics; + /** Swerve Module locations. */ + public Translation2d[] moduleLocationsMeters; + /** Swerve IMU */ + public SwerveIMU imu; + /** Invert the imu measurements. */ + public boolean invertedIMU = false; + /** Number of modules on the robot. */ + public int moduleCount; + /** Swerve Modules. */ + public SwerveModule[] modules; + /** Physical characteristics of the swerve drive from physicalproperties.json. */ + public SwerveModulePhysicalCharacteristics physicalCharacteristics; - /** - * Create swerve drive configuration. - * - * @param moduleConfigs Module configuration. - * @param swerveIMU Swerve IMU. - * @param invertedIMU Invert the IMU. - * @param driveFeedforward The drive motor feedforward to use for the {@link SwerveModule}. - * @param physicalCharacteristics {@link SwerveModulePhysicalCharacteristics} to store in association with self. - */ - public SwerveDriveConfiguration( - SwerveModuleConfiguration[] moduleConfigs, - SwerveIMU swerveIMU, - boolean invertedIMU, - SimpleMotorFeedforward driveFeedforward, - SwerveModulePhysicalCharacteristics physicalCharacteristics) - { - this.moduleCount = moduleConfigs.length; - this.imu = swerveIMU; - this.invertedIMU = invertedIMU; - this.modules = createModules(moduleConfigs, driveFeedforward); - this.moduleLocationsMeters = new Translation2d[moduleConfigs.length]; - for (SwerveModule module : modules) - { - this.moduleLocationsMeters[module.moduleNumber] = module.configuration.moduleLocation; - } - this.physicalCharacteristics = physicalCharacteristics; - } + /** + * Create swerve drive configuration. + * + * @param moduleConfigs Module configuration. + * @param swerveIMU Swerve IMU. + * @param invertedIMU Invert the IMU. + * @param driveFeedforward The drive motor feedforward to use for the {@link SwerveModule}. + * @param physicalCharacteristics {@link SwerveModulePhysicalCharacteristics} to store in + * association with self. + */ + public SwerveDriveConfiguration( + SwerveModuleConfiguration[] moduleConfigs, + SwerveIMU swerveIMU, + boolean invertedIMU, + SimpleMotorFeedforward driveFeedforward, + SwerveModulePhysicalCharacteristics physicalCharacteristics) { + this.moduleCount = moduleConfigs.length; + this.imu = swerveIMU; + this.invertedIMU = invertedIMU; + this.modules = createModules(moduleConfigs, driveFeedforward); + this.moduleLocationsMeters = new Translation2d[moduleConfigs.length]; + for (SwerveModule module : modules) { + this.moduleLocationsMeters[module.moduleNumber] = module.configuration.moduleLocation; + } + this.physicalCharacteristics = physicalCharacteristics; + } - /** - * Create modules based off of the SwerveModuleConfiguration. - * - * @param swerves Swerve constants. - * @param driveFeedforward Drive feedforward created using - * {@link swervelib.math.SwerveMath#createDriveFeedforward(double, double, double)}. - * @return Swerve Modules. - */ - public SwerveModule[] createModules(SwerveModuleConfiguration[] swerves, SimpleMotorFeedforward driveFeedforward) - { - SwerveModule[] modArr = new SwerveModule[swerves.length]; - for (int i = 0; i < swerves.length; i++) - { - modArr[i] = new SwerveModule(i, swerves[i], driveFeedforward); - } - return modArr; - } + /** + * Create modules based off of the SwerveModuleConfiguration. + * + * @param swerves Swerve constants. + * @param driveFeedforward Drive feedforward created using {@link + * swervelib.math.SwerveMath#createDriveFeedforward(double, double, double)}. + * @return Swerve Modules. + */ + public SwerveModule[] createModules( + SwerveModuleConfiguration[] swerves, SimpleMotorFeedforward driveFeedforward) { + SwerveModule[] modArr = new SwerveModule[swerves.length]; + for (int i = 0; i < swerves.length; i++) { + modArr[i] = new SwerveModule(i, swerves[i], driveFeedforward); + } + return modArr; + } - /** - * Assume the first module is the furthest. Usually front-left. - * - * @return Drive base radius from center of robot to the farthest wheel in meters. - */ - public double getDriveBaseRadiusMeters() - { - Translation2d furthestModule = moduleLocationsMeters[0]; - return Math.abs(Math.sqrt(Math.pow(furthestModule.getX(), 2) + Math.pow(furthestModule.getY(), 2))); - } + /** + * Assume the first module is the furthest. Usually front-left. + * + * @return Drive base radius from center of robot to the farthest wheel in meters. + */ + public double getDriveBaseRadiusMeters() { + Translation2d furthestModule = moduleLocationsMeters[0]; + return Math.abs( + Math.sqrt(Math.pow(furthestModule.getX(), 2) + Math.pow(furthestModule.getY(), 2))); + } } diff --git a/src/main/java/swervelib/parser/SwerveModuleConfiguration.java b/src/main/java/swervelib/parser/SwerveModuleConfiguration.java index 2e8d9f89..3d5c6b1f 100644 --- a/src/main/java/swervelib/parser/SwerveModuleConfiguration.java +++ b/src/main/java/swervelib/parser/SwerveModuleConfiguration.java @@ -5,158 +5,129 @@ import swervelib.motors.SwerveMotor; import swervelib.parser.json.MotorConfigDouble; -/** - * Swerve Module configuration class which is used to configure {@link swervelib.SwerveModule}. - */ -public class SwerveModuleConfiguration -{ +/** Swerve Module configuration class which is used to configure {@link swervelib.SwerveModule}. */ +public class SwerveModuleConfiguration { - /** - * Conversion factor for drive motor onboard PID's and angle PID's. Use - * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} and - * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)} respectively to calculate the - * conversion factors. - */ - public final MotorConfigDouble conversionFactors; - /** - * Angle offset in degrees for the Swerve Module. - */ - public final double angleOffset; - /** - * Whether the absolute encoder is inverted. - */ - public final boolean absoluteEncoderInverted; - /** - * State of inversion of the drive motor. - */ - public final boolean driveMotorInverted; - /** - * State of inversion of the angle motor. - */ - public final boolean angleMotorInverted; - /** - * PIDF configuration options for the angle motor closed-loop PID controller. - */ - public PIDFConfig anglePIDF; - /** - * PIDF configuration options for the drive motor closed-loop PID controller. - */ - public PIDFConfig velocityPIDF; - /** - * Swerve module location relative to the robot. - */ - public Translation2d moduleLocation; - /** - * Physical characteristics of the swerve module. - */ - public SwerveModulePhysicalCharacteristics physicalCharacteristics; - /** - * The drive motor and angle motor of this swerve module. - */ - public SwerveMotor driveMotor, angleMotor; - /** - * The Absolute Encoder for the swerve module. - */ - public SwerveAbsoluteEncoder absoluteEncoder; - /** - * Name for the swerve module for telemetry. - */ - public String name; - - /** - * Construct a configuration object for swerve modules. - * - * @param driveMotor Drive {@link SwerveMotor}. - * @param angleMotor Angle {@link SwerveMotor} - * @param absoluteEncoder Absolute encoder {@link SwerveAbsoluteEncoder}. - * @param angleOffset Absolute angle offset to 0. - * @param absoluteEncoderInverted Absolute encoder inverted. - * @param angleMotorInverted State of inversion of the angle motor. - * @param driveMotorInverted Drive motor inverted. - * @param xMeters Module location in meters from the center horizontally. - * @param yMeters Module location in meters from center vertically. - * @param anglePIDF Angle PIDF configuration. - * @param velocityPIDF Velocity PIDF configuration. - * @param physicalCharacteristics Physical characteristics of the swerve module. - * @param name The name for the swerve module. - * @param conversionFactors Conversion factors to be applied to the drive and angle motors. - */ - public SwerveModuleConfiguration( - SwerveMotor driveMotor, - SwerveMotor angleMotor, - MotorConfigDouble conversionFactors, - SwerveAbsoluteEncoder absoluteEncoder, - double angleOffset, - double xMeters, - double yMeters, - PIDFConfig anglePIDF, - PIDFConfig velocityPIDF, - SwerveModulePhysicalCharacteristics physicalCharacteristics, - boolean absoluteEncoderInverted, - boolean driveMotorInverted, - boolean angleMotorInverted, - String name) - { - this.driveMotor = driveMotor; - this.angleMotor = angleMotor; - this.conversionFactors = conversionFactors; - this.absoluteEncoder = absoluteEncoder; - this.angleOffset = angleOffset; - this.absoluteEncoderInverted = absoluteEncoderInverted; - this.driveMotorInverted = driveMotorInverted; - this.angleMotorInverted = angleMotorInverted; - this.moduleLocation = new Translation2d(xMeters, yMeters); - this.anglePIDF = anglePIDF; - this.velocityPIDF = velocityPIDF; - this.physicalCharacteristics = physicalCharacteristics; - this.name = name; - } - - /** - * Construct a configuration object for swerve modules. Assumes the absolute encoder and drive motor are not - * inverted. - * - * @param driveMotor Drive {@link SwerveMotor}. - * @param angleMotor Angle {@link SwerveMotor} - * @param conversionFactors Conversion factors for angle/azimuth motors drive factors. - * @param absoluteEncoder Absolute encoder {@link SwerveAbsoluteEncoder}. - * @param angleOffset Absolute angle offset to 0. - * @param xMeters Module location in meters from the center horizontally. - * @param yMeters Module location in meters from center vertically. - * @param anglePIDF Angle PIDF configuration. - * @param velocityPIDF Velocity PIDF configuration. - * @param physicalCharacteristics Physical characteristics of the swerve module. - * @param name Name for the module. - */ - public SwerveModuleConfiguration( - SwerveMotor driveMotor, - SwerveMotor angleMotor, - MotorConfigDouble conversionFactors, - SwerveAbsoluteEncoder absoluteEncoder, - double angleOffset, - double xMeters, - double yMeters, - PIDFConfig anglePIDF, - PIDFConfig velocityPIDF, - SwerveModulePhysicalCharacteristics physicalCharacteristics, - String name) - { - this( - driveMotor, - angleMotor, - conversionFactors, - absoluteEncoder, - angleOffset, - xMeters, - yMeters, - anglePIDF, - velocityPIDF, - physicalCharacteristics, - false, - false, - false, - name); - } + /** + * Conversion factor for drive motor onboard PID's and angle PID's. Use {@link + * swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} and {@link + * swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)} respectively to + * calculate the conversion factors. + */ + public final MotorConfigDouble conversionFactors; + /** Angle offset in degrees for the Swerve Module. */ + public final double angleOffset; + /** Whether the absolute encoder is inverted. */ + public final boolean absoluteEncoderInverted; + /** State of inversion of the drive motor. */ + public final boolean driveMotorInverted; + /** State of inversion of the angle motor. */ + public final boolean angleMotorInverted; + /** PIDF configuration options for the angle motor closed-loop PID controller. */ + public PIDFConfig anglePIDF; + /** PIDF configuration options for the drive motor closed-loop PID controller. */ + public PIDFConfig velocityPIDF; + /** Swerve module location relative to the robot. */ + public Translation2d moduleLocation; + /** Physical characteristics of the swerve module. */ + public SwerveModulePhysicalCharacteristics physicalCharacteristics; + /** The drive motor and angle motor of this swerve module. */ + public SwerveMotor driveMotor, angleMotor; + /** The Absolute Encoder for the swerve module. */ + public SwerveAbsoluteEncoder absoluteEncoder; + /** Name for the swerve module for telemetry. */ + public String name; + /** + * Construct a configuration object for swerve modules. + * + * @param driveMotor Drive {@link SwerveMotor}. + * @param angleMotor Angle {@link SwerveMotor} + * @param absoluteEncoder Absolute encoder {@link SwerveAbsoluteEncoder}. + * @param angleOffset Absolute angle offset to 0. + * @param absoluteEncoderInverted Absolute encoder inverted. + * @param angleMotorInverted State of inversion of the angle motor. + * @param driveMotorInverted Drive motor inverted. + * @param xMeters Module location in meters from the center horizontally. + * @param yMeters Module location in meters from center vertically. + * @param anglePIDF Angle PIDF configuration. + * @param velocityPIDF Velocity PIDF configuration. + * @param physicalCharacteristics Physical characteristics of the swerve module. + * @param name The name for the swerve module. + * @param conversionFactors Conversion factors to be applied to the drive and angle motors. + */ + public SwerveModuleConfiguration( + SwerveMotor driveMotor, + SwerveMotor angleMotor, + MotorConfigDouble conversionFactors, + SwerveAbsoluteEncoder absoluteEncoder, + double angleOffset, + double xMeters, + double yMeters, + PIDFConfig anglePIDF, + PIDFConfig velocityPIDF, + SwerveModulePhysicalCharacteristics physicalCharacteristics, + boolean absoluteEncoderInverted, + boolean driveMotorInverted, + boolean angleMotorInverted, + String name) { + this.driveMotor = driveMotor; + this.angleMotor = angleMotor; + this.conversionFactors = conversionFactors; + this.absoluteEncoder = absoluteEncoder; + this.angleOffset = angleOffset; + this.absoluteEncoderInverted = absoluteEncoderInverted; + this.driveMotorInverted = driveMotorInverted; + this.angleMotorInverted = angleMotorInverted; + this.moduleLocation = new Translation2d(xMeters, yMeters); + this.anglePIDF = anglePIDF; + this.velocityPIDF = velocityPIDF; + this.physicalCharacteristics = physicalCharacteristics; + this.name = name; + } + /** + * Construct a configuration object for swerve modules. Assumes the absolute encoder and drive + * motor are not inverted. + * + * @param driveMotor Drive {@link SwerveMotor}. + * @param angleMotor Angle {@link SwerveMotor} + * @param conversionFactors Conversion factors for angle/azimuth motors drive factors. + * @param absoluteEncoder Absolute encoder {@link SwerveAbsoluteEncoder}. + * @param angleOffset Absolute angle offset to 0. + * @param xMeters Module location in meters from the center horizontally. + * @param yMeters Module location in meters from center vertically. + * @param anglePIDF Angle PIDF configuration. + * @param velocityPIDF Velocity PIDF configuration. + * @param physicalCharacteristics Physical characteristics of the swerve module. + * @param name Name for the module. + */ + public SwerveModuleConfiguration( + SwerveMotor driveMotor, + SwerveMotor angleMotor, + MotorConfigDouble conversionFactors, + SwerveAbsoluteEncoder absoluteEncoder, + double angleOffset, + double xMeters, + double yMeters, + PIDFConfig anglePIDF, + PIDFConfig velocityPIDF, + SwerveModulePhysicalCharacteristics physicalCharacteristics, + String name) { + this( + driveMotor, + angleMotor, + conversionFactors, + absoluteEncoder, + angleOffset, + xMeters, + yMeters, + anglePIDF, + velocityPIDF, + physicalCharacteristics, + false, + false, + false, + name); + } } diff --git a/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java b/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java index 99360ce1..11439164 100644 --- a/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java +++ b/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java @@ -2,105 +2,80 @@ import swervelib.parser.json.MotorConfigDouble; -/** - * Configuration class which stores physical characteristics shared between every swerve module. - */ -public class SwerveModulePhysicalCharacteristics -{ +/** Configuration class which stores physical characteristics shared between every swerve module. */ +public class SwerveModulePhysicalCharacteristics { - /** - * Current limits for the Swerve Module. - */ - public final int driveMotorCurrentLimit, angleMotorCurrentLimit; - /** - * The time it takes for the motor to go from 0 to full throttle in seconds. - */ - public final double driveMotorRampRate, angleMotorRampRate; - /** - * Wheel grip tape coefficient of friction on carpet, as described by the vendor. - */ - public final double wheelGripCoefficientOfFriction; - /** - * The voltage to use for the smart motor voltage compensation. - */ - public double optimalVoltage; - /** - * The conversion factors for the drive and angle motors, created by - * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} and - * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)}. - */ - public MotorConfigDouble conversionFactor; + /** Current limits for the Swerve Module. */ + public final int driveMotorCurrentLimit, angleMotorCurrentLimit; + /** The time it takes for the motor to go from 0 to full throttle in seconds. */ + public final double driveMotorRampRate, angleMotorRampRate; + /** Wheel grip tape coefficient of friction on carpet, as described by the vendor. */ + public final double wheelGripCoefficientOfFriction; + /** The voltage to use for the smart motor voltage compensation. */ + public double optimalVoltage; + /** + * The conversion factors for the drive and angle motors, created by {@link + * swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} and {@link + * swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)}. + */ + public MotorConfigDouble conversionFactor; - /** - * Construct the swerve module physical characteristics. - * - * @param conversionFactors The conversion factors for the drive and angle motors, created by - * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, - * double)} and - * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, - * double)}. - * @param wheelGripCoefficientOfFriction Wheel grip coefficient of friction on carpet given by manufacturer. - * @param optimalVoltage Optimal robot voltage. - * @param driveMotorCurrentLimit Current limit for the drive motor. - * @param angleMotorCurrentLimit Current limit for the angle motor. - * @param driveMotorRampRate The time in seconds to go from 0 to full throttle on the motor. (Prevents - * over drawing power from battery) - * @param angleMotorRampRate The time in seconds to go from 0 to full throttle on the motor. (Prevents - * overdrawing power and power loss). - */ - public SwerveModulePhysicalCharacteristics( - MotorConfigDouble conversionFactors, - double wheelGripCoefficientOfFriction, - double optimalVoltage, - int driveMotorCurrentLimit, - int angleMotorCurrentLimit, - double driveMotorRampRate, - double angleMotorRampRate) - { - this.wheelGripCoefficientOfFriction = wheelGripCoefficientOfFriction; - this.optimalVoltage = optimalVoltage; + /** + * Construct the swerve module physical characteristics. + * + * @param conversionFactors The conversion factors for the drive and angle motors, created by + * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} and + * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)}. + * @param wheelGripCoefficientOfFriction Wheel grip coefficient of friction on carpet given by + * manufacturer. + * @param optimalVoltage Optimal robot voltage. + * @param driveMotorCurrentLimit Current limit for the drive motor. + * @param angleMotorCurrentLimit Current limit for the angle motor. + * @param driveMotorRampRate The time in seconds to go from 0 to full throttle on the motor. + * (Prevents over drawing power from battery) + * @param angleMotorRampRate The time in seconds to go from 0 to full throttle on the motor. + * (Prevents overdrawing power and power loss). + */ + public SwerveModulePhysicalCharacteristics( + MotorConfigDouble conversionFactors, + double wheelGripCoefficientOfFriction, + double optimalVoltage, + int driveMotorCurrentLimit, + int angleMotorCurrentLimit, + double driveMotorRampRate, + double angleMotorRampRate) { + this.wheelGripCoefficientOfFriction = wheelGripCoefficientOfFriction; + this.optimalVoltage = optimalVoltage; - this.conversionFactor = conversionFactors; - // Set the conversion factors to null if they are both 0. - if (conversionFactors != null) - { - if (conversionFactors.angle == 0 && conversionFactors.drive == 0) - { - this.conversionFactor = null; - } - } + this.conversionFactor = conversionFactors; + // Set the conversion factors to null if they are both 0. + if (conversionFactors != null) { + if (conversionFactors.angle == 0 && conversionFactors.drive == 0) { + this.conversionFactor = null; + } + } - this.driveMotorCurrentLimit = driveMotorCurrentLimit; - this.angleMotorCurrentLimit = angleMotorCurrentLimit; - this.driveMotorRampRate = driveMotorRampRate; - this.angleMotorRampRate = angleMotorRampRate; - } + this.driveMotorCurrentLimit = driveMotorCurrentLimit; + this.angleMotorCurrentLimit = angleMotorCurrentLimit; + this.driveMotorRampRate = driveMotorRampRate; + this.angleMotorRampRate = angleMotorRampRate; + } - /** - * Construct the swerve module physical characteristics. Assume coefficient of friction is 1.19 (taken from blue - * nitrile on carpet from Studica) and optimal voltage is 12v. Assumes the drive motor current limit is 40A, and the - * angle motor current limit is 20A. - * - * @param conversionFactors The conversion factors for the drive and angle motors, created by - * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} and - * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)}. - * @param driveMotorRampRate The time in seconds to go from 0 to full throttle on the motor. (Prevents over drawing - * power from battery) - * @param angleMotorRampRate The time in seconds to go from 0 to full throttle on the motor. (Prevents overdrawing - * power and power loss). - */ - public SwerveModulePhysicalCharacteristics( - MotorConfigDouble conversionFactors, - double driveMotorRampRate, - double angleMotorRampRate) - { - this( - conversionFactors, - 1.19, - 12, - 40, - 20, - driveMotorRampRate, - angleMotorRampRate); - } + /** + * Construct the swerve module physical characteristics. Assume coefficient of friction is 1.19 + * (taken from blue nitrile on carpet from Studica) and optimal voltage is 12v. Assumes the drive + * motor current limit is 40A, and the angle motor current limit is 20A. + * + * @param conversionFactors The conversion factors for the drive and angle motors, created by + * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} and + * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)}. + * @param driveMotorRampRate The time in seconds to go from 0 to full throttle on the motor. + * (Prevents over drawing power from battery) + * @param angleMotorRampRate The time in seconds to go from 0 to full throttle on the motor. + * (Prevents overdrawing power and power loss). + */ + public SwerveModulePhysicalCharacteristics( + MotorConfigDouble conversionFactors, double driveMotorRampRate, double angleMotorRampRate) { + this(conversionFactors, 1.19, 12, 40, 20, driveMotorRampRate, angleMotorRampRate); + } } diff --git a/src/main/java/swervelib/parser/SwerveParser.java b/src/main/java/swervelib/parser/SwerveParser.java index 573fdd87..3c4517f6 100644 --- a/src/main/java/swervelib/parser/SwerveParser.java +++ b/src/main/java/swervelib/parser/SwerveParser.java @@ -15,213 +15,194 @@ import swervelib.parser.json.PhysicalPropertiesJson; import swervelib.parser.json.SwerveDriveJson; -/** - * Helper class used to parse the JSON directory with specified configuration options. - */ -public class SwerveParser -{ +/** Helper class used to parse the JSON directory with specified configuration options. */ +public class SwerveParser { - /** - * Module number mapped to the JSON name. - */ - private static final HashMap moduleConfigs = new HashMap<>(); - /** - * Parsed swervedrive.json - */ - public static SwerveDriveJson swerveDriveJson; - /** - * Parsed controllerproperties.json - */ - public static ControllerPropertiesJson controllerPropertiesJson; - /** - * Parsed modules/pidfproperties.json - */ - public static PIDFPropertiesJson pidfPropertiesJson; - /** - * Parsed modules/physicalproperties.json - */ - public static PhysicalPropertiesJson physicalPropertiesJson; - /** - * Array holding the module jsons given in {@link SwerveDriveJson}. - */ - public static ModuleJson[] moduleJsons; + /** Module number mapped to the JSON name. */ + private static final HashMap moduleConfigs = new HashMap<>(); + /** Parsed swervedrive.json */ + public static SwerveDriveJson swerveDriveJson; + /** Parsed controllerproperties.json */ + public static ControllerPropertiesJson controllerPropertiesJson; + /** Parsed modules/pidfproperties.json */ + public static PIDFPropertiesJson pidfPropertiesJson; + /** Parsed modules/physicalproperties.json */ + public static PhysicalPropertiesJson physicalPropertiesJson; + /** Array holding the module jsons given in {@link SwerveDriveJson}. */ + public static ModuleJson[] moduleJsons; - /** - * Construct a swerve parser. Will throw an error if there is a missing file. - * - * @param directory Directory with swerve configurations. - * @throws IOException if a file doesn't exist. - */ - public SwerveParser(File directory) throws IOException - { - checkDirectory(directory); - swerveDriveJson = - new ObjectMapper() - .readValue(new File(directory, "swervedrive.json"), SwerveDriveJson.class); - controllerPropertiesJson = - new ObjectMapper() - .readValue( - new File(directory, "controllerproperties.json"), ControllerPropertiesJson.class); - pidfPropertiesJson = - new ObjectMapper() - .readValue( - new File(directory, "modules/pidfproperties.json"), PIDFPropertiesJson.class); - physicalPropertiesJson = - new ObjectMapper() - .readValue( - new File(directory, "modules/physicalproperties.json"), - PhysicalPropertiesJson.class); - moduleJsons = new ModuleJson[swerveDriveJson.modules.length]; - for (int i = 0; i < moduleJsons.length; i++) - { - moduleConfigs.put(swerveDriveJson.modules[i], i); - File moduleFile = new File(directory, "modules/" + swerveDriveJson.modules[i]); - assert moduleFile.exists(); - moduleJsons[i] = new ObjectMapper().readValue(moduleFile, ModuleJson.class); - } - } + /** + * Construct a swerve parser. Will throw an error if there is a missing file. + * + * @param directory Directory with swerve configurations. + * @throws IOException if a file doesn't exist. + */ + public SwerveParser(File directory) throws IOException { + checkDirectory(directory); + swerveDriveJson = + new ObjectMapper() + .readValue(new File(directory, "swervedrive.json"), SwerveDriveJson.class); + controllerPropertiesJson = + new ObjectMapper() + .readValue( + new File(directory, "controllerproperties.json"), ControllerPropertiesJson.class); + pidfPropertiesJson = + new ObjectMapper() + .readValue( + new File(directory, "modules/pidfproperties.json"), PIDFPropertiesJson.class); + physicalPropertiesJson = + new ObjectMapper() + .readValue( + new File(directory, "modules/physicalproperties.json"), + PhysicalPropertiesJson.class); + moduleJsons = new ModuleJson[swerveDriveJson.modules.length]; + for (int i = 0; i < moduleJsons.length; i++) { + moduleConfigs.put(swerveDriveJson.modules[i], i); + File moduleFile = new File(directory, "modules/" + swerveDriveJson.modules[i]); + assert moduleFile.exists(); + moduleJsons[i] = new ObjectMapper().readValue(moduleFile, ModuleJson.class); + } + } - /** - * Get the swerve module by the json name. - * - * @param name JSON name. - * @param driveConfiguration {@link SwerveDriveConfiguration} to pull from. - * @return {@link SwerveModuleConfiguration} based on the file. - */ - public static SwerveModule getModuleConfigurationByName( - String name, SwerveDriveConfiguration driveConfiguration) - { - return driveConfiguration.modules[moduleConfigs.get(name + ".json")]; - } + /** + * Get the swerve module by the json name. + * + * @param name JSON name. + * @param driveConfiguration {@link SwerveDriveConfiguration} to pull from. + * @return {@link SwerveModuleConfiguration} based on the file. + */ + public static SwerveModule getModuleConfigurationByName( + String name, SwerveDriveConfiguration driveConfiguration) { + return driveConfiguration.modules[moduleConfigs.get(name + ".json")]; + } - /** - * Open JSON file. - * - * @param file JSON File to open. - * @return JsonNode of file. - */ - private JsonNode openJson(File file) - { - try - { - return new ObjectMapper().readTree(file); - } catch (IOException e) - { - throw new RuntimeException(e); - } - } + /** + * Open JSON file. + * + * @param file JSON File to open. + * @return JsonNode of file. + */ + private JsonNode openJson(File file) { + try { + return new ObjectMapper().readTree(file); + } catch (IOException e) { + throw new RuntimeException(e); + } + } - /** - * Check directory structure. - * - * @param directory JSON Configuration Directory - */ - private void checkDirectory(File directory) - { - assert new File(directory, "swervedrive.json").exists(); - assert new File(directory, "controllerproperties.json").exists(); - assert new File(directory, "modules").exists() && new File(directory, "modules").isDirectory(); - assert new File(directory, "modules/pidfproperties.json").exists(); - assert new File(directory, "modules/physicalproperties.json").exists(); - } + /** + * Check directory structure. + * + * @param directory JSON Configuration Directory + */ + private void checkDirectory(File directory) { + assert new File(directory, "swervedrive.json").exists(); + assert new File(directory, "controllerproperties.json").exists(); + assert new File(directory, "modules").exists() && new File(directory, "modules").isDirectory(); + assert new File(directory, "modules/pidfproperties.json").exists(); + assert new File(directory, "modules/physicalproperties.json").exists(); + } - /** - * Create {@link SwerveDrive} from JSON configuration directory. - * - * @param maxSpeed Maximum speed of the robot in meters per second, used for both angular acceleration used in - * {@link swervelib.SwerveController} and drive feedforward in - * {@link SwerveMath#createDriveFeedforward(double, double, double)}. - * @return {@link SwerveDrive} instance. - */ - public SwerveDrive createSwerveDrive(double maxSpeed) - { - return createSwerveDrive(SwerveMath.createDriveFeedforward(physicalPropertiesJson.optimalVoltage, - maxSpeed, - physicalPropertiesJson.wheelGripCoefficientOfFriction), - maxSpeed); - } + /** + * Create {@link SwerveDrive} from JSON configuration directory. + * + * @param maxSpeed Maximum speed of the robot in meters per second, used for both angular + * acceleration used in {@link swervelib.SwerveController} and drive feedforward in {@link + * SwerveMath#createDriveFeedforward(double, double, double)}. + * @return {@link SwerveDrive} instance. + */ + public SwerveDrive createSwerveDrive(double maxSpeed) { + return createSwerveDrive( + SwerveMath.createDriveFeedforward( + physicalPropertiesJson.optimalVoltage, + maxSpeed, + physicalPropertiesJson.wheelGripCoefficientOfFriction), + maxSpeed); + } - /** - * Create {@link SwerveDrive} from JSON configuration directory. - * - * @param maxSpeed Maximum speed of the robot in meters per second, used for both angular - * acceleration used in {@link swervelib.SwerveController} and drive feedforward in - * {@link SwerveMath#createDriveFeedforward(double, double, double)}. - * @param angleMotorConversionFactor Angle (AKA azimuth) motor conversion factor to convert motor controller PID loop - * units to degrees, usually created using - * {@link SwerveMath#calculateDegreesPerSteeringRotation(double, double)}. - * @param driveMotorConversion Drive motor conversion factor to convert motor controller PID loop units to - * meters per rotation, usually created using - * {@link SwerveMath#calculateMetersPerRotation(double, double, double)}. - * @return {@link SwerveDrive} instance. - */ - public SwerveDrive createSwerveDrive(double maxSpeed, double angleMotorConversionFactor, double driveMotorConversion) - { - physicalPropertiesJson.conversionFactor.angle = angleMotorConversionFactor; - physicalPropertiesJson.conversionFactor.drive = driveMotorConversion; - return createSwerveDrive(SwerveMath.createDriveFeedforward(physicalPropertiesJson.optimalVoltage, - maxSpeed, - physicalPropertiesJson.wheelGripCoefficientOfFriction), - maxSpeed); - } + /** + * Create {@link SwerveDrive} from JSON configuration directory. + * + * @param maxSpeed Maximum speed of the robot in meters per second, used for both angular + * acceleration used in {@link swervelib.SwerveController} and drive feedforward in {@link + * SwerveMath#createDriveFeedforward(double, double, double)}. + * @param angleMotorConversionFactor Angle (AKA azimuth) motor conversion factor to convert motor + * controller PID loop units to degrees, usually created using {@link + * SwerveMath#calculateDegreesPerSteeringRotation(double, double)}. + * @param driveMotorConversion Drive motor conversion factor to convert motor controller PID loop + * units to meters per rotation, usually created using {@link + * SwerveMath#calculateMetersPerRotation(double, double, double)}. + * @return {@link SwerveDrive} instance. + */ + public SwerveDrive createSwerveDrive( + double maxSpeed, double angleMotorConversionFactor, double driveMotorConversion) { + physicalPropertiesJson.conversionFactor.angle = angleMotorConversionFactor; + physicalPropertiesJson.conversionFactor.drive = driveMotorConversion; + return createSwerveDrive( + SwerveMath.createDriveFeedforward( + physicalPropertiesJson.optimalVoltage, + maxSpeed, + physicalPropertiesJson.wheelGripCoefficientOfFriction), + maxSpeed); + } - /** - * Create {@link SwerveDrive} from JSON configuration directory. - * - * @param driveFeedforward Drive feedforward to use for swerve modules, should be created using - * {@link swervelib.math.SwerveMath#createDriveFeedforward(double, double, - * double)}. - * @param maxSpeed Maximum speed of the robot in meters per second for normal+angular acceleration - * in {@link swervelib.SwerveController} of the robot. - * @param angleMotorConversionFactor Angle (AKA azimuth) motor conversion factor to convert motor controller PID loop - * units to degrees, usually created using - * {@link SwerveMath#calculateDegreesPerSteeringRotation(double, double)}. - * @param driveMotorConversion Drive motor conversion factor to convert motor controller PID loop units to - * meters per rotation, usually created using - * {@link SwerveMath#calculateMetersPerRotation(double, double, double)}. - * @return {@link SwerveDrive} instance. - */ - public SwerveDrive createSwerveDrive(SimpleMotorFeedforward driveFeedforward, double maxSpeed, - double angleMotorConversionFactor, double driveMotorConversion) - { - physicalPropertiesJson.conversionFactor.angle = angleMotorConversionFactor; - physicalPropertiesJson.conversionFactor.drive = driveMotorConversion; - return createSwerveDrive(driveFeedforward, maxSpeed); - } + /** + * Create {@link SwerveDrive} from JSON configuration directory. + * + * @param driveFeedforward Drive feedforward to use for swerve modules, should be created using + * {@link swervelib.math.SwerveMath#createDriveFeedforward(double, double, double)}. + * @param maxSpeed Maximum speed of the robot in meters per second for normal+angular acceleration + * in {@link swervelib.SwerveController} of the robot. + * @param angleMotorConversionFactor Angle (AKA azimuth) motor conversion factor to convert motor + * controller PID loop units to degrees, usually created using {@link + * SwerveMath#calculateDegreesPerSteeringRotation(double, double)}. + * @param driveMotorConversion Drive motor conversion factor to convert motor controller PID loop + * units to meters per rotation, usually created using {@link + * SwerveMath#calculateMetersPerRotation(double, double, double)}. + * @return {@link SwerveDrive} instance. + */ + public SwerveDrive createSwerveDrive( + SimpleMotorFeedforward driveFeedforward, + double maxSpeed, + double angleMotorConversionFactor, + double driveMotorConversion) { + physicalPropertiesJson.conversionFactor.angle = angleMotorConversionFactor; + physicalPropertiesJson.conversionFactor.drive = driveMotorConversion; + return createSwerveDrive(driveFeedforward, maxSpeed); + } - /** - * Create {@link SwerveDrive} from JSON configuration directory. - * - * @param driveFeedforward Drive feedforward to use for swerve modules, should be created using - * {@link swervelib.math.SwerveMath#createDriveFeedforward(double, double, double)}. - * @param maxSpeed Maximum speed of the robot in meters per second for normal+angular acceleration in - * {@link swervelib.SwerveController} of the robot - * @return {@link SwerveDrive} instance. - */ - public SwerveDrive createSwerveDrive(SimpleMotorFeedforward driveFeedforward, double maxSpeed) - { - SwerveModuleConfiguration[] moduleConfigurations = - new SwerveModuleConfiguration[moduleJsons.length]; - for (int i = 0; i < moduleConfigurations.length; i++) - { - ModuleJson module = moduleJsons[i]; - moduleConfigurations[i] = - module.createModuleConfiguration( - pidfPropertiesJson.angle, - pidfPropertiesJson.drive, - physicalPropertiesJson.createPhysicalProperties(), - swerveDriveJson.modules[i]); - } - SwerveDriveConfiguration swerveDriveConfiguration = - new SwerveDriveConfiguration( - moduleConfigurations, - swerveDriveJson.imu.createIMU(), - swerveDriveJson.invertedIMU, - driveFeedforward, - physicalPropertiesJson.createPhysicalProperties()); + /** + * Create {@link SwerveDrive} from JSON configuration directory. + * + * @param driveFeedforward Drive feedforward to use for swerve modules, should be created using + * {@link swervelib.math.SwerveMath#createDriveFeedforward(double, double, double)}. + * @param maxSpeed Maximum speed of the robot in meters per second for normal+angular acceleration + * in {@link swervelib.SwerveController} of the robot + * @return {@link SwerveDrive} instance. + */ + public SwerveDrive createSwerveDrive(SimpleMotorFeedforward driveFeedforward, double maxSpeed) { + SwerveModuleConfiguration[] moduleConfigurations = + new SwerveModuleConfiguration[moduleJsons.length]; + for (int i = 0; i < moduleConfigurations.length; i++) { + ModuleJson module = moduleJsons[i]; + moduleConfigurations[i] = + module.createModuleConfiguration( + pidfPropertiesJson.angle, + pidfPropertiesJson.drive, + physicalPropertiesJson.createPhysicalProperties(), + swerveDriveJson.modules[i]); + } + SwerveDriveConfiguration swerveDriveConfiguration = + new SwerveDriveConfiguration( + moduleConfigurations, + swerveDriveJson.imu.createIMU(), + swerveDriveJson.invertedIMU, + driveFeedforward, + physicalPropertiesJson.createPhysicalProperties()); - return new SwerveDrive( - swerveDriveConfiguration, - controllerPropertiesJson.createControllerConfiguration(swerveDriveConfiguration, maxSpeed), maxSpeed); - } + return new SwerveDrive( + swerveDriveConfiguration, + controllerPropertiesJson.createControllerConfiguration(swerveDriveConfiguration, maxSpeed), + maxSpeed); + } } diff --git a/src/main/java/swervelib/parser/deserializer/PIDFRange.java b/src/main/java/swervelib/parser/deserializer/PIDFRange.java index c90f2312..91f8b591 100644 --- a/src/main/java/swervelib/parser/deserializer/PIDFRange.java +++ b/src/main/java/swervelib/parser/deserializer/PIDFRange.java @@ -1,17 +1,10 @@ package swervelib.parser.deserializer; -/** - * Class to hold the minimum and maximum input or output of the PIDF. - */ -public class PIDFRange -{ +/** Class to hold the minimum and maximum input or output of the PIDF. */ +public class PIDFRange { - /** - * Minimum value. - */ - public double min = -1; - /** - * Maximum value. - */ - public double max = 1; + /** Minimum value. */ + public double min = -1; + /** Maximum value. */ + public double max = 1; } diff --git a/src/main/java/swervelib/parser/deserializer/package-info.java b/src/main/java/swervelib/parser/deserializer/package-info.java index 820f2cc0..8a176651 100644 --- a/src/main/java/swervelib/parser/deserializer/package-info.java +++ b/src/main/java/swervelib/parser/deserializer/package-info.java @@ -1,4 +1,2 @@ -/** - * Deserialize specific variables for outside the parser. - */ +/** Deserialize specific variables for outside the parser. */ package swervelib.parser.deserializer; diff --git a/src/main/java/swervelib/parser/json/ControllerPropertiesJson.java b/src/main/java/swervelib/parser/json/ControllerPropertiesJson.java index 103107db..42f90090 100644 --- a/src/main/java/swervelib/parser/json/ControllerPropertiesJson.java +++ b/src/main/java/swervelib/parser/json/ControllerPropertiesJson.java @@ -4,32 +4,27 @@ import swervelib.parser.SwerveControllerConfiguration; import swervelib.parser.SwerveDriveConfiguration; -/** - * {@link swervelib.SwerveController} parsed class. Used to access the JSON data. - */ -public class ControllerPropertiesJson -{ +/** {@link swervelib.SwerveController} parsed class. Used to access the JSON data. */ +public class ControllerPropertiesJson { - /** - * The minimum radius of the angle control joystick to allow for heading adjustment of the robot. - */ - public double angleJoystickRadiusDeadband; - /** - * The PID used to control the robot heading. - */ - public PIDFConfig heading; + /** + * The minimum radius of the angle control joystick to allow for heading adjustment of the robot. + */ + public double angleJoystickRadiusDeadband; + /** The PID used to control the robot heading. */ + public PIDFConfig heading; - /** - * Create the {@link SwerveControllerConfiguration} based on parsed and given data. - * - * @param driveConfiguration {@link SwerveDriveConfiguration} parsed configuration. - * @param maxSpeedMPS Maximum speed in meters per second for the angular acceleration of the robot. - * @return {@link SwerveControllerConfiguration} object based on parsed data. - */ - public SwerveControllerConfiguration createControllerConfiguration( - SwerveDriveConfiguration driveConfiguration, double maxSpeedMPS) - { - return new SwerveControllerConfiguration( - driveConfiguration, heading, angleJoystickRadiusDeadband, maxSpeedMPS); - } + /** + * Create the {@link SwerveControllerConfiguration} based on parsed and given data. + * + * @param driveConfiguration {@link SwerveDriveConfiguration} parsed configuration. + * @param maxSpeedMPS Maximum speed in meters per second for the angular acceleration of the + * robot. + * @return {@link SwerveControllerConfiguration} object based on parsed data. + */ + public SwerveControllerConfiguration createControllerConfiguration( + SwerveDriveConfiguration driveConfiguration, double maxSpeedMPS) { + return new SwerveControllerConfiguration( + driveConfiguration, heading, angleJoystickRadiusDeadband, maxSpeedMPS); + } } diff --git a/src/main/java/swervelib/parser/json/DeviceJson.java b/src/main/java/swervelib/parser/json/DeviceJson.java index 9b3a8aac..67363aef 100644 --- a/src/main/java/swervelib/parser/json/DeviceJson.java +++ b/src/main/java/swervelib/parser/json/DeviceJson.java @@ -27,164 +27,149 @@ import swervelib.motors.TalonFXSwerve; import swervelib.motors.TalonSRXSwerve; -/** - * Device JSON parsed class. Used to access the JSON data. - */ -public class DeviceJson -{ +/** Device JSON parsed class. Used to access the JSON data. */ +public class DeviceJson { - /** - * The device type, e.g. pigeon/pigeon2/sparkmax/talonfx/navx - */ - public String type; - /** - * The CAN ID or pin ID of the device. - */ - public int id; - /** - * The CAN bus name which the device resides on if using CAN. - */ - public String canbus = ""; + /** The device type, e.g. pigeon/pigeon2/sparkmax/talonfx/navx */ + public String type; + /** The CAN ID or pin ID of the device. */ + public int id; + /** The CAN bus name which the device resides on if using CAN. */ + public String canbus = ""; - /** - * Create a {@link SwerveAbsoluteEncoder} from the current configuration. - * - * @param motor {@link SwerveMotor} of which attached encoders will be created from, only used when the type is - * "attached" or "canandencoder". - * @return {@link SwerveAbsoluteEncoder} given. - */ - public SwerveAbsoluteEncoder createEncoder(SwerveMotor motor) - { - if (id > 40) - { - DriverStation.reportWarning("CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!", - false); - } - switch (type) - { - case "none": - return null; - case "integrated": - case "attached": - return new SparkMaxEncoderSwerve(motor, 1); - case "sparkmax_analog": - return new SparkMaxAnalogEncoderSwerve(motor); - case "canandcoder": - return new SparkMaxEncoderSwerve(motor, 360); - case "canandcoder_can": - return new CanAndCoderSwerve(id); - case "ma3": - case "ctre_mag": - case "rev_hex": - case "throughbore": - case "am_mag": - case "dutycycle": - return new PWMDutyCycleEncoderSwerve(id); - case "thrifty": - case "analog": - return new AnalogAbsoluteEncoderSwerve(id); - case "cancoder": - return new CANCoderSwerve(id, canbus != null ? canbus : ""); - default: - throw new RuntimeException(type + " is not a recognized absolute encoder type."); - } - } + /** + * Create a {@link SwerveAbsoluteEncoder} from the current configuration. + * + * @param motor {@link SwerveMotor} of which attached encoders will be created from, only used + * when the type is "attached" or "canandencoder". + * @return {@link SwerveAbsoluteEncoder} given. + */ + public SwerveAbsoluteEncoder createEncoder(SwerveMotor motor) { + if (id > 40) { + DriverStation.reportWarning( + "CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!", + false); + } + switch (type) { + case "none": + return null; + case "integrated": + case "attached": + return new SparkMaxEncoderSwerve(motor, 1); + case "sparkmax_analog": + return new SparkMaxAnalogEncoderSwerve(motor); + case "canandcoder": + return new SparkMaxEncoderSwerve(motor, 360); + case "canandcoder_can": + return new CanAndCoderSwerve(id); + case "ma3": + case "ctre_mag": + case "rev_hex": + case "throughbore": + case "am_mag": + case "dutycycle": + return new PWMDutyCycleEncoderSwerve(id); + case "thrifty": + case "analog": + return new AnalogAbsoluteEncoderSwerve(id); + case "cancoder": + return new CANCoderSwerve(id, canbus != null ? canbus : ""); + default: + throw new RuntimeException(type + " is not a recognized absolute encoder type."); + } + } - /** - * Create a {@link SwerveIMU} from the given configuration. - * - * @return {@link SwerveIMU} given. - */ - public SwerveIMU createIMU() - { - if (id > 40) - { - DriverStation.reportWarning("CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!", - false); - } - switch (type) - { - case "adis16448": - return new ADIS16448Swerve(); - case "adis16470": - return new ADIS16470Swerve(); - case "adxrs450": - return new ADXRS450Swerve(); - case "analog": - return new AnalogGyroSwerve(id); - case "navx": - case "navx_spi": - return new NavXSwerve(SPI.Port.kMXP); - case "navx_i2c": - DriverStation.reportWarning( - "WARNING: There exists an I2C lockup issue on the roboRIO that could occur, more information here: " + - "\nhttps://docs.wpilib.org/en/stable/docs/yearly-overview/known-issues" + - ".html#onboard-i2c-causing-system-lockups", - false); - return new NavXSwerve(I2C.Port.kMXP); - case "navx_usb": - return new NavXSwerve(Port.kUSB); - case "navx_mxp": - return new NavXSwerve(Port.kMXP); - case "pigeon": - return new PigeonSwerve(id); - case "pigeon2": - return new Pigeon2Swerve(id, canbus != null ? canbus : ""); - default: - throw new RuntimeException(type + " is not a recognized imu/gyroscope type."); - } - } + /** + * Create a {@link SwerveIMU} from the given configuration. + * + * @return {@link SwerveIMU} given. + */ + public SwerveIMU createIMU() { + if (id > 40) { + DriverStation.reportWarning( + "CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!", + false); + } + switch (type) { + case "adis16448": + return new ADIS16448Swerve(); + case "adis16470": + return new ADIS16470Swerve(); + case "adxrs450": + return new ADXRS450Swerve(); + case "analog": + return new AnalogGyroSwerve(id); + case "navx": + case "navx_spi": + return new NavXSwerve(SPI.Port.kMXP); + case "navx_i2c": + DriverStation.reportWarning( + "WARNING: There exists an I2C lockup issue on the roboRIO that could occur, more information here: " + + "\nhttps://docs.wpilib.org/en/stable/docs/yearly-overview/known-issues" + + ".html#onboard-i2c-causing-system-lockups", + false); + return new NavXSwerve(I2C.Port.kMXP); + case "navx_usb": + return new NavXSwerve(Port.kUSB); + case "navx_mxp": + return new NavXSwerve(Port.kMXP); + case "pigeon": + return new PigeonSwerve(id); + case "pigeon2": + return new Pigeon2Swerve(id, canbus != null ? canbus : ""); + default: + throw new RuntimeException(type + " is not a recognized imu/gyroscope type."); + } + } - /** - * Create a {@link SwerveMotor} from the given configuration. - * - * @param isDriveMotor If the motor being generated is a drive motor. - * @return {@link SwerveMotor} given. - */ - public SwerveMotor createMotor(boolean isDriveMotor) - { - if (id > 40) - { - DriverStation.reportWarning("CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!", - false); - } - switch (type) - { - case "sparkmax_brushed": - switch (canbus) - { - case "greyhill_63r256": - return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 1024, false); - case "srx_mag_encoder": - return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 4096, false); - case "throughbore": - return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 8192, false); - case "throughbore_dataport": - return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kNoSensor, 8192, true); - case "greyhill_63r256_dataport": - return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 1024, true); - case "srx_mag_encoder_dataport": - return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 4096, true); - default: - if (isDriveMotor) - { - throw new RuntimeException("Spark MAX " + id + " MUST have a encoder attached to the motor controller."); - } - // We are creating a motor for an angle motor which will use the absolute encoder attached to the data port. - return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kNoSensor, 0, false); - } - case "neo": - case "sparkmax": - return new SparkMaxSwerve(id, isDriveMotor); - case "sparkflex": - return new SparkFlexSwerve(id, isDriveMotor); - case "falcon": - case "talonfx": - return new TalonFXSwerve(id, canbus != null ? canbus : "", isDriveMotor); - case "talonsrx": - return new TalonSRXSwerve(id, isDriveMotor); - default: - throw new RuntimeException(type + " is not a recognized motor type."); - } - } + /** + * Create a {@link SwerveMotor} from the given configuration. + * + * @param isDriveMotor If the motor being generated is a drive motor. + * @return {@link SwerveMotor} given. + */ + public SwerveMotor createMotor(boolean isDriveMotor) { + if (id > 40) { + DriverStation.reportWarning( + "CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!", + false); + } + switch (type) { + case "sparkmax_brushed": + switch (canbus) { + case "greyhill_63r256": + return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 1024, false); + case "srx_mag_encoder": + return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 4096, false); + case "throughbore": + return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 8192, false); + case "throughbore_dataport": + return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kNoSensor, 8192, true); + case "greyhill_63r256_dataport": + return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 1024, true); + case "srx_mag_encoder_dataport": + return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 4096, true); + default: + if (isDriveMotor) { + throw new RuntimeException( + "Spark MAX " + id + " MUST have a encoder attached to the motor controller."); + } + // We are creating a motor for an angle motor which will use the absolute encoder + // attached to the data port. + return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kNoSensor, 0, false); + } + case "neo": + case "sparkmax": + return new SparkMaxSwerve(id, isDriveMotor); + case "sparkflex": + return new SparkFlexSwerve(id, isDriveMotor); + case "falcon": + case "talonfx": + return new TalonFXSwerve(id, canbus != null ? canbus : "", isDriveMotor); + case "talonsrx": + return new TalonSRXSwerve(id, isDriveMotor); + default: + throw new RuntimeException(type + " is not a recognized motor type."); + } + } } diff --git a/src/main/java/swervelib/parser/json/ModuleJson.java b/src/main/java/swervelib/parser/json/ModuleJson.java index 0d4f4906..ef6debcf 100644 --- a/src/main/java/swervelib/parser/json/ModuleJson.java +++ b/src/main/java/swervelib/parser/json/ModuleJson.java @@ -11,127 +11,108 @@ import swervelib.parser.json.modules.BoolMotorJson; import swervelib.parser.json.modules.LocationJson; -/** - * {@link swervelib.SwerveModule} JSON parsed class. Used to access the JSON data. - */ -public class ModuleJson -{ +/** {@link swervelib.SwerveModule} JSON parsed class. Used to access the JSON data. */ +public class ModuleJson { - /** - * Drive motor device configuration. - */ - public DeviceJson drive; - /** - * Angle motor device configuration. - */ - public DeviceJson angle; - /** - * Conversion factor for the module, if different from the one in swervedrive.json - *

- * Conversion factor applied to the motor controllers PID loops. Can be calculated with - * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)} for angle motors or - * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} for drive motors. - */ - public MotorConfigDouble conversionFactor = new MotorConfigDouble(0, 0); - /** - * Absolute encoder device configuration. - */ - public DeviceJson encoder; - /** - * Defines which motors are inverted. - */ - public BoolMotorJson inverted; - /** - * Absolute encoder offset from 0 in degrees. - */ - public double absoluteEncoderOffset; - /** - * Absolute encoder inversion state. - */ - public boolean absoluteEncoderInverted = false; - /** - * The location of the swerve module from the center of the robot in inches. - */ - public LocationJson location; + /** Drive motor device configuration. */ + public DeviceJson drive; + /** Angle motor device configuration. */ + public DeviceJson angle; + /** + * Conversion factor for the module, if different from the one in swervedrive.json + * + *

Conversion factor applied to the motor controllers PID loops. Can be calculated with {@link + * swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)} for angle motors + * or {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} for + * drive motors. + */ + public MotorConfigDouble conversionFactor = new MotorConfigDouble(0, 0); + /** Absolute encoder device configuration. */ + public DeviceJson encoder; + /** Defines which motors are inverted. */ + public BoolMotorJson inverted; + /** Absolute encoder offset from 0 in degrees. */ + public double absoluteEncoderOffset; + /** Absolute encoder inversion state. */ + public boolean absoluteEncoderInverted = false; + /** The location of the swerve module from the center of the robot in inches. */ + public LocationJson location; - /** - * Create the swerve module configuration based off of parsed data. - * - * @param anglePIDF The PIDF values for the angle motor. - * @param velocityPIDF The velocity PIDF values for the drive motor. - * @param physicalCharacteristics Physical characteristics of the swerve module. - * @param name Module json filename. - * @return {@link SwerveModuleConfiguration} based on the provided data and parsed data. - */ - public SwerveModuleConfiguration createModuleConfiguration( - PIDFConfig anglePIDF, - PIDFConfig velocityPIDF, - SwerveModulePhysicalCharacteristics physicalCharacteristics, - String name) - { - SwerveMotor angleMotor = angle.createMotor(false); - SwerveAbsoluteEncoder absEncoder = encoder.createEncoder(angleMotor); + /** + * Create the swerve module configuration based off of parsed data. + * + * @param anglePIDF The PIDF values for the angle motor. + * @param velocityPIDF The velocity PIDF values for the drive motor. + * @param physicalCharacteristics Physical characteristics of the swerve module. + * @param name Module json filename. + * @return {@link SwerveModuleConfiguration} based on the provided data and parsed data. + */ + public SwerveModuleConfiguration createModuleConfiguration( + PIDFConfig anglePIDF, + PIDFConfig velocityPIDF, + SwerveModulePhysicalCharacteristics physicalCharacteristics, + String name) { + SwerveMotor angleMotor = angle.createMotor(false); + SwerveAbsoluteEncoder absEncoder = encoder.createEncoder(angleMotor); - // If the absolute encoder is attached. - if (absEncoder != null && angleMotor.getMotor() instanceof CANSparkMax) - { - if (absEncoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) - { - angleMotor.setAbsoluteEncoder(absEncoder); - } - } + // If the absolute encoder is attached. + if (absEncoder != null && angleMotor.getMotor() instanceof CANSparkMax) { + if (absEncoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) { + angleMotor.setAbsoluteEncoder(absEncoder); + } + } - // Set the conversion factors to null if they are both 0. - if (this.conversionFactor != null) - { - if (this.conversionFactor.angle == 0 && this.conversionFactor.drive == 0) - { - this.conversionFactor = null; - } - } + // Set the conversion factors to null if they are both 0. + if (this.conversionFactor != null) { + if (this.conversionFactor.angle == 0 && this.conversionFactor.drive == 0) { + this.conversionFactor = null; + } + } - if (this.conversionFactor == null && physicalCharacteristics.conversionFactor == null) - { - throw new RuntimeException("No Conversion Factor configured! Please create SwerveDrive using \n" + - "SwerveParser.createSwerveDrive(driveFeedforward, maxSpeed, angleMotorConversionFactor, driveMotorConversion)\n" + - "OR\n" + - "SwerveParser.createSwerveDrive(maxSpeed, angleMotorConversionFactor, driveMotorConversion)\n" + - "OR\n" + - "Set the conversion factor in physicalproperties.json OR the module JSON file." + - "REMEMBER: You can calculate the conversion factors using SwerveMath.calculateMetersPerRotation AND SwerveMath.calculateDegreesPerSteeringRotation\n"); - } else if (physicalCharacteristics.conversionFactor != null && this.conversionFactor == null) - { - this.conversionFactor = physicalCharacteristics.conversionFactor; - } else if (physicalCharacteristics.conversionFactor != - null) // If both are defined, override 0 with the physical characterstics input. - { - this.conversionFactor.angle = this.conversionFactor.angle == 0 ? physicalCharacteristics.conversionFactor.angle - : this.conversionFactor.angle; - this.conversionFactor.drive = this.conversionFactor.drive == 0 ? physicalCharacteristics.conversionFactor.drive - : this.conversionFactor.drive; - } + if (this.conversionFactor == null && physicalCharacteristics.conversionFactor == null) { + throw new RuntimeException( + "No Conversion Factor configured! Please create SwerveDrive using \n" + + "SwerveParser.createSwerveDrive(driveFeedforward, maxSpeed, angleMotorConversionFactor, driveMotorConversion)\n" + + "OR\n" + + "SwerveParser.createSwerveDrive(maxSpeed, angleMotorConversionFactor, driveMotorConversion)\n" + + "OR\n" + + "Set the conversion factor in physicalproperties.json OR the module JSON file." + + "REMEMBER: You can calculate the conversion factors using SwerveMath.calculateMetersPerRotation AND SwerveMath.calculateDegreesPerSteeringRotation\n"); + } else if (physicalCharacteristics.conversionFactor != null && this.conversionFactor == null) { + this.conversionFactor = physicalCharacteristics.conversionFactor; + } else if (physicalCharacteristics.conversionFactor + != null) // If both are defined, override 0 with the physical characterstics input. + { + this.conversionFactor.angle = + this.conversionFactor.angle == 0 + ? physicalCharacteristics.conversionFactor.angle + : this.conversionFactor.angle; + this.conversionFactor.drive = + this.conversionFactor.drive == 0 + ? physicalCharacteristics.conversionFactor.drive + : this.conversionFactor.drive; + } - if (this.conversionFactor.drive == 0 || this.conversionFactor.angle == 0) - { - throw new RuntimeException( - "Conversion factors cannot be 0, please configure conversion factors in physicalproperties.json or the module JSON files."); - } - System.out.println(conversionFactor.drive); + if (this.conversionFactor.drive == 0 || this.conversionFactor.angle == 0) { + throw new RuntimeException( + "Conversion factors cannot be 0, please configure conversion factors in physicalproperties.json or the module JSON files."); + } + System.out.println(conversionFactor.drive); - return new SwerveModuleConfiguration( - drive.createMotor(true), - angleMotor, - conversionFactor, - absEncoder, - absoluteEncoderOffset, - Units.inchesToMeters(Math.round(location.x) == 0 ? location.front : location.x), - Units.inchesToMeters(Math.round(location.y) == 0 ? location.left : location.y), - anglePIDF, - velocityPIDF, - physicalCharacteristics, - absoluteEncoderInverted, - inverted.drive, - inverted.angle, - name.replaceAll("\\.json", "")); - } + return new SwerveModuleConfiguration( + drive.createMotor(true), + angleMotor, + conversionFactor, + absEncoder, + absoluteEncoderOffset, + Units.inchesToMeters(Math.round(location.x) == 0 ? location.front : location.x), + Units.inchesToMeters(Math.round(location.y) == 0 ? location.left : location.y), + anglePIDF, + velocityPIDF, + physicalCharacteristics, + absoluteEncoderInverted, + inverted.drive, + inverted.angle, + name.replaceAll("\\.json", "")); + } } diff --git a/src/main/java/swervelib/parser/json/MotorConfigDouble.java b/src/main/java/swervelib/parser/json/MotorConfigDouble.java index 86766b3b..9e137b50 100644 --- a/src/main/java/swervelib/parser/json/MotorConfigDouble.java +++ b/src/main/java/swervelib/parser/json/MotorConfigDouble.java @@ -1,36 +1,24 @@ package swervelib.parser.json; -/** - * Used to store doubles for motor configuration. - */ -public class MotorConfigDouble -{ +/** Used to store doubles for motor configuration. */ +public class MotorConfigDouble { - /** - * Drive motor. - */ - public double drive; - /** - * Angle motor. - */ - public double angle; + /** Drive motor. */ + public double drive; + /** Angle motor. */ + public double angle; - /** - * Default constructor. - */ - public MotorConfigDouble() - { - } + /** Default constructor. */ + public MotorConfigDouble() {} - /** - * Default constructor. - * - * @param angle Angle data. - * @param drive Drive data. - */ - public MotorConfigDouble(double angle, double drive) - { - this.angle = angle; - this.drive = drive; - } + /** + * Default constructor. + * + * @param angle Angle data. + * @param drive Drive data. + */ + public MotorConfigDouble(double angle, double drive) { + this.angle = angle; + this.drive = drive; + } } diff --git a/src/main/java/swervelib/parser/json/MotorConfigInt.java b/src/main/java/swervelib/parser/json/MotorConfigInt.java index d31726c1..66b314f0 100644 --- a/src/main/java/swervelib/parser/json/MotorConfigInt.java +++ b/src/main/java/swervelib/parser/json/MotorConfigInt.java @@ -1,36 +1,24 @@ package swervelib.parser.json; -/** - * Used to store ints for motor configuration. - */ -public class MotorConfigInt -{ +/** Used to store ints for motor configuration. */ +public class MotorConfigInt { - /** - * Drive motor. - */ - public int drive; - /** - * Angle motor. - */ - public int angle; + /** Drive motor. */ + public int drive; + /** Angle motor. */ + public int angle; - /** - * Default constructor. - */ - public MotorConfigInt() - { - } + /** Default constructor. */ + public MotorConfigInt() {} - /** - * Default constructor with values. - * - * @param drive Drive data. - * @param angle Angle data. - */ - public MotorConfigInt(int drive, int angle) - { - this.angle = angle; - this.drive = drive; - } + /** + * Default constructor with values. + * + * @param drive Drive data. + * @param angle Angle data. + */ + public MotorConfigInt(int drive, int angle) { + this.angle = angle; + this.drive = drive; + } } diff --git a/src/main/java/swervelib/parser/json/PIDFPropertiesJson.java b/src/main/java/swervelib/parser/json/PIDFPropertiesJson.java index 9dd9dc0c..68ab9a99 100644 --- a/src/main/java/swervelib/parser/json/PIDFPropertiesJson.java +++ b/src/main/java/swervelib/parser/json/PIDFPropertiesJson.java @@ -2,18 +2,11 @@ import swervelib.parser.PIDFConfig; -/** - * {@link swervelib.SwerveModule} PID with Feedforward for the drive motor and angle motor. - */ -public class PIDFPropertiesJson -{ +/** {@link swervelib.SwerveModule} PID with Feedforward for the drive motor and angle motor. */ +public class PIDFPropertiesJson { - /** - * The PIDF with Integral Zone used for the drive motor. - */ - public PIDFConfig drive; - /** - * The PIDF with Integral Zone used for the angle motor. - */ - public PIDFConfig angle; + /** The PIDF with Integral Zone used for the drive motor. */ + public PIDFConfig drive; + /** The PIDF with Integral Zone used for the angle motor. */ + public PIDFConfig angle; } diff --git a/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java b/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java index 4ede4025..0d6877b6 100644 --- a/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java +++ b/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java @@ -3,50 +3,43 @@ import swervelib.parser.SwerveModulePhysicalCharacteristics; /** - * {@link swervelib.parser.SwerveModulePhysicalCharacteristics} parsed data. Used to configure the SwerveModule. + * {@link swervelib.parser.SwerveModulePhysicalCharacteristics} parsed data. Used to configure the + * SwerveModule. */ -public class PhysicalPropertiesJson -{ +public class PhysicalPropertiesJson { + /** + * Conversion factor applied to the motor controllers PID loops. Can be calculated with {@link + * swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)} for angle motors + * or {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} for + * drive motors. + */ + public MotorConfigDouble conversionFactor = new MotorConfigDouble(0, 0); + /** The current limit in AMPs to apply to the motors. */ + public MotorConfigInt currentLimit = new MotorConfigInt(40, 20); + /** The minimum number of seconds to take for the motor to go from 0 to full throttle. */ + public MotorConfigDouble rampRate = new MotorConfigDouble(0.25, 0.25); + /** + * The grip tape coefficient of friction on carpet. Used to calculate the practical maximum + * acceleration. + */ + public double wheelGripCoefficientOfFriction = 1.19; + /** The voltage to use for the smart motor voltage compensation, default is 12. */ + public double optimalVoltage = 12; - /** - * Conversion factor applied to the motor controllers PID loops. Can be calculated with - * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)} for angle motors or - * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} for drive motors. - */ - public MotorConfigDouble conversionFactor = new MotorConfigDouble(0, 0); - /** - * The current limit in AMPs to apply to the motors. - */ - public MotorConfigInt currentLimit = new MotorConfigInt(40, 20); - /** - * The minimum number of seconds to take for the motor to go from 0 to full throttle. - */ - public MotorConfigDouble rampRate = new MotorConfigDouble(0.25, 0.25); - /** - * The grip tape coefficient of friction on carpet. Used to calculate the practical maximum acceleration. - */ - public double wheelGripCoefficientOfFriction = 1.19; - /** - * The voltage to use for the smart motor voltage compensation, default is 12. - */ - public double optimalVoltage = 12; - - /** - * Create the physical characteristics based off the parsed data. - * - * @return {@link SwerveModulePhysicalCharacteristics} based on parsed data. - */ - public SwerveModulePhysicalCharacteristics createPhysicalProperties() - { - return new SwerveModulePhysicalCharacteristics( - conversionFactor, - wheelGripCoefficientOfFriction, - optimalVoltage, - currentLimit.drive, - currentLimit.angle, - rampRate.drive, - rampRate.angle); - } + /** + * Create the physical characteristics based off the parsed data. + * + * @return {@link SwerveModulePhysicalCharacteristics} based on parsed data. + */ + public SwerveModulePhysicalCharacteristics createPhysicalProperties() { + return new SwerveModulePhysicalCharacteristics( + conversionFactor, + wheelGripCoefficientOfFriction, + optimalVoltage, + currentLimit.drive, + currentLimit.angle, + rampRate.drive, + rampRate.angle); + } } - diff --git a/src/main/java/swervelib/parser/json/SwerveDriveJson.java b/src/main/java/swervelib/parser/json/SwerveDriveJson.java index 332d30d7..a31a9eff 100644 --- a/src/main/java/swervelib/parser/json/SwerveDriveJson.java +++ b/src/main/java/swervelib/parser/json/SwerveDriveJson.java @@ -1,21 +1,15 @@ package swervelib.parser.json; /** - * {@link swervelib.SwerveDrive} JSON parsed class. Used to access parsed data from the swervedrive.json file. + * {@link swervelib.SwerveDrive} JSON parsed class. Used to access parsed data from the + * swervedrive.json file. */ -public class SwerveDriveJson -{ +public class SwerveDriveJson { - /** - * Robot IMU used to determine heading of the robot. - */ - public DeviceJson imu; - /** - * Invert the IMU of the robot. - */ - public boolean invertedIMU; - /** - * Module JSONs in order clockwise order starting from front left. - */ - public String[] modules; + /** Robot IMU used to determine heading of the robot. */ + public DeviceJson imu; + /** Invert the IMU of the robot. */ + public boolean invertedIMU; + /** Module JSONs in order clockwise order starting from front left. */ + public String[] modules; } diff --git a/src/main/java/swervelib/parser/json/modules/BoolMotorJson.java b/src/main/java/swervelib/parser/json/modules/BoolMotorJson.java index f6d603f5..f80ad941 100644 --- a/src/main/java/swervelib/parser/json/modules/BoolMotorJson.java +++ b/src/main/java/swervelib/parser/json/modules/BoolMotorJson.java @@ -1,17 +1,10 @@ package swervelib.parser.json.modules; -/** - * Inverted motor JSON parsed class. Used to access the JSON data. - */ -public class BoolMotorJson -{ +/** Inverted motor JSON parsed class. Used to access the JSON data. */ +public class BoolMotorJson { - /** - * Drive motor inversion state. - */ - public boolean drive; - /** - * Angle motor inversion state. - */ - public boolean angle; + /** Drive motor inversion state. */ + public boolean drive; + /** Angle motor inversion state. */ + public boolean angle; } diff --git a/src/main/java/swervelib/parser/json/modules/LocationJson.java b/src/main/java/swervelib/parser/json/modules/LocationJson.java index a0471efb..fbc3381b 100644 --- a/src/main/java/swervelib/parser/json/modules/LocationJson.java +++ b/src/main/java/swervelib/parser/json/modules/LocationJson.java @@ -1,18 +1,14 @@ package swervelib.parser.json.modules; /** - * Location JSON parsed class. Used to access the JSON data. Module locations, in inches, as distances to the center of - * the robot. +x is towards the robot front, and +y is towards robot left. + * Location JSON parsed class. Used to access the JSON data. Module locations, in inches, as + * distances to the center of the robot. +x is towards the robot front, and +y is towards robot + * left. */ -public class LocationJson -{ +public class LocationJson { - /** - * Location of the swerve module in inches from the center of the robot horizontally. - */ - public double front = 0, x = 0; - /** - * Location of the swerve module in inches from the center of the robot vertically. - */ - public double left = 0, y = 0; + /** Location of the swerve module in inches from the center of the robot horizontally. */ + public double front = 0, x = 0; + /** Location of the swerve module in inches from the center of the robot vertically. */ + public double left = 0, y = 0; } diff --git a/src/main/java/swervelib/parser/json/modules/package-info.java b/src/main/java/swervelib/parser/json/modules/package-info.java index b5ff2b71..f6e0ab73 100644 --- a/src/main/java/swervelib/parser/json/modules/package-info.java +++ b/src/main/java/swervelib/parser/json/modules/package-info.java @@ -1,4 +1,2 @@ -/** - * JSON Mapped Configuration types for modules. - */ +/** JSON Mapped Configuration types for modules. */ package swervelib.parser.json.modules; diff --git a/src/main/java/swervelib/parser/json/package-info.java b/src/main/java/swervelib/parser/json/package-info.java index 836fbc0e..776e4150 100644 --- a/src/main/java/swervelib/parser/json/package-info.java +++ b/src/main/java/swervelib/parser/json/package-info.java @@ -1,4 +1,2 @@ -/** - * JSON Mapped classes for parsing configuration files. - */ +/** JSON Mapped classes for parsing configuration files. */ package swervelib.parser.json; diff --git a/src/main/java/swervelib/parser/package-info.java b/src/main/java/swervelib/parser/package-info.java index ade6ed38..60372be5 100644 --- a/src/main/java/swervelib/parser/package-info.java +++ b/src/main/java/swervelib/parser/package-info.java @@ -1,4 +1,2 @@ -/** - * JSON Parser for YAGSL configurations. - */ +/** JSON Parser for YAGSL configurations. */ package swervelib.parser; diff --git a/src/main/java/swervelib/simulation/SwerveIMUSimulation.java b/src/main/java/swervelib/simulation/SwerveIMUSimulation.java index b3f2b62d..9efdf35b 100644 --- a/src/main/java/swervelib/simulation/SwerveIMUSimulation.java +++ b/src/main/java/swervelib/simulation/SwerveIMUSimulation.java @@ -10,113 +10,95 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d; import java.util.Optional; -/** - * Simulation for {@link swervelib.SwerveDrive} IMU. - */ -public class SwerveIMUSimulation -{ +/** Simulation for {@link swervelib.SwerveDrive} IMU. */ +public class SwerveIMUSimulation { - /** - * Main timer to control movement estimations. - */ - private final Timer timer; - /** - * The last time the timer was read, used to determine position changes. - */ - private double lastTime; - /** - * Heading of the robot. - */ - private double angle; + /** Main timer to control movement estimations. */ + private final Timer timer; + /** The last time the timer was read, used to determine position changes. */ + private double lastTime; + /** Heading of the robot. */ + private double angle; - /** - * Create the swerve drive IMU simulation. - */ - public SwerveIMUSimulation() - { - timer = new Timer(); - timer.start(); - lastTime = timer.get(); - } + /** Create the swerve drive IMU simulation. */ + public SwerveIMUSimulation() { + timer = new Timer(); + timer.start(); + lastTime = timer.get(); + } - /** - * Get the estimated angle of the robot. - * - * @return {@link Rotation2d} estimation of the robot. - */ - public Rotation2d getYaw() - { - return new Rotation2d(angle); - } + /** + * Get the estimated angle of the robot. + * + * @return {@link Rotation2d} estimation of the robot. + */ + public Rotation2d getYaw() { + return new Rotation2d(angle); + } - /** - * Pitch is not simulated currently, always returns 0. - * - * @return Pitch of the robot as {@link Rotation2d}. - */ - public Rotation2d getPitch() - { - return new Rotation2d(); - } + /** + * Pitch is not simulated currently, always returns 0. + * + * @return Pitch of the robot as {@link Rotation2d}. + */ + public Rotation2d getPitch() { + return new Rotation2d(); + } - /** - * Roll is not simulated currently, always returns 0. - * - * @return Roll of the robot as {@link Rotation2d}. - */ - public Rotation2d getRoll() - { - return new Rotation2d(); - } + /** + * Roll is not simulated currently, always returns 0. + * + * @return Roll of the robot as {@link Rotation2d}. + */ + public Rotation2d getRoll() { + return new Rotation2d(); + } - /** - * Gets the estimated gyro {@link Rotation3d} of the robot. - * - * @return The heading as a {@link Rotation3d} angle - */ - public Rotation3d getGyroRotation3d() - { - return new Rotation3d(0, 0, angle); - } + /** + * Gets the estimated gyro {@link Rotation3d} of the robot. + * + * @return The heading as a {@link Rotation3d} angle + */ + public Rotation3d getGyroRotation3d() { + return new Rotation3d(0, 0, angle); + } - /** - * Fetch the acceleration [x, y, z] from the IMU in m/s/s. If acceleration isn't supported returns empty. - * - * @return {@link Translation3d} of the acceleration as an {@link Optional}. - */ - public Optional getAccel() - { - return Optional.empty(); - } + /** + * Fetch the acceleration [x, y, z] from the IMU in m/s/s. If acceleration isn't supported returns + * empty. + * + * @return {@link Translation3d} of the acceleration as an {@link Optional}. + */ + public Optional getAccel() { + return Optional.empty(); + } - /** - * Update the odometry of the simulated {@link swervelib.SwerveDrive} and post the {@link swervelib.SwerveModule} - * states to the {@link Field2d}. - * - * @param kinematics {@link SwerveDriveKinematics} of the swerve drive. - * @param states {@link SwerveModuleState} array of the module states. - * @param modulePoses {@link Pose2d} representing the swerve modules. - * @param field {@link Field2d} to update. - */ - public void updateOdometry( - SwerveDriveKinematics kinematics, - SwerveModuleState[] states, - Pose2d[] modulePoses, - Field2d field) - { + /** + * Update the odometry of the simulated {@link swervelib.SwerveDrive} and post the {@link + * swervelib.SwerveModule} states to the {@link Field2d}. + * + * @param kinematics {@link SwerveDriveKinematics} of the swerve drive. + * @param states {@link SwerveModuleState} array of the module states. + * @param modulePoses {@link Pose2d} representing the swerve modules. + * @param field {@link Field2d} to update. + */ + public void updateOdometry( + SwerveDriveKinematics kinematics, + SwerveModuleState[] states, + Pose2d[] modulePoses, + Field2d field) { - angle += kinematics.toChassisSpeeds(states).omegaRadiansPerSecond * (timer.get() - lastTime); - lastTime = timer.get(); - field.getObject("XModules").setPoses(modulePoses); - } + angle += kinematics.toChassisSpeeds(states).omegaRadiansPerSecond * (timer.get() - lastTime); + lastTime = timer.get(); + field.getObject("XModules").setPoses(modulePoses); + } - /** - * Set the heading of the robot. - * - * @param angle Angle of the robot in radians. - */ - public void setAngle(double angle) - { - this.angle = angle; - } + /** + * Set the heading of the robot. + * + * @param angle Angle of the robot in radians. + */ + public void setAngle(double angle) { + this.angle = angle; + } } diff --git a/src/main/java/swervelib/simulation/SwerveModuleSimulation.java b/src/main/java/swervelib/simulation/SwerveModuleSimulation.java index 2fb06f8f..c75df5af 100644 --- a/src/main/java/swervelib/simulation/SwerveModuleSimulation.java +++ b/src/main/java/swervelib/simulation/SwerveModuleSimulation.java @@ -5,87 +5,66 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.Timer; -/** - * Class to hold simulation data for {@link swervelib.SwerveModule} - */ -public class SwerveModuleSimulation -{ +/** Class to hold simulation data for {@link swervelib.SwerveModule} */ +public class SwerveModuleSimulation { + /** Main timer to simulate the passage of time. */ + private final Timer timer; + /** Time delta since last update */ + private double dt; + /** Fake motor position. */ + private double fakePos; + /** + * The fake speed of the previous state, used to calculate {@link SwerveModuleSimulation#fakePos}. + */ + private double fakeSpeed; + /** Last time queried. */ + private double lastTime; + /** Current simulated swerve module state. */ + private SwerveModuleState state; - /** - * Main timer to simulate the passage of time. - */ - private final Timer timer; - /** - * Time delta since last update - */ - private double dt; - /** - * Fake motor position. - */ - private double fakePos; - /** - * The fake speed of the previous state, used to calculate {@link SwerveModuleSimulation#fakePos}. - */ - private double fakeSpeed; - /** - * Last time queried. - */ - private double lastTime; - /** - * Current simulated swerve module state. - */ - private SwerveModuleState state; + /** Create simulation class and initialize module at 0. */ + public SwerveModuleSimulation() { + timer = new Timer(); + timer.start(); + lastTime = timer.get(); + state = new SwerveModuleState(0, Rotation2d.fromDegrees(0)); + fakeSpeed = 0; + fakePos = 0; + dt = 0; + } - /** - * Create simulation class and initialize module at 0. - */ - public SwerveModuleSimulation() - { - timer = new Timer(); - timer.start(); - lastTime = timer.get(); - state = new SwerveModuleState(0, Rotation2d.fromDegrees(0)); - fakeSpeed = 0; - fakePos = 0; - dt = 0; - } + /** + * Update the position and state of the module. Called from {@link + * swervelib.SwerveModule#setDesiredState} function when simulated. + * + * @param desiredState State the swerve module is set to. + */ + public void updateStateAndPosition(SwerveModuleState desiredState) { + dt = timer.get() - lastTime; + lastTime = timer.get(); - /** - * Update the position and state of the module. Called from {@link swervelib.SwerveModule#setDesiredState} function - * when simulated. - * - * @param desiredState State the swerve module is set to. - */ - public void updateStateAndPosition(SwerveModuleState desiredState) - { - dt = timer.get() - lastTime; - lastTime = timer.get(); + state = desiredState; + fakeSpeed = desiredState.speedMetersPerSecond; + fakePos += (fakeSpeed * dt); + } - state = desiredState; - fakeSpeed = desiredState.speedMetersPerSecond; - fakePos += (fakeSpeed * dt); + /** + * Get the simulated swerve module position. + * + * @return {@link SwerveModulePosition} of the simulated module. + */ + public SwerveModulePosition getPosition() { - } + return new SwerveModulePosition(fakePos, state.angle); + } - /** - * Get the simulated swerve module position. - * - * @return {@link SwerveModulePosition} of the simulated module. - */ - public SwerveModulePosition getPosition() - { - - return new SwerveModulePosition(fakePos, state.angle); - } - - /** - * Get the {@link SwerveModuleState} of the simulated module. - * - * @return {@link SwerveModuleState} of the simulated module. - */ - public SwerveModuleState getState() - { - return state; - } + /** + * Get the {@link SwerveModuleState} of the simulated module. + * + * @return {@link SwerveModuleState} of the simulated module. + */ + public SwerveModuleState getState() { + return state; + } } diff --git a/src/main/java/swervelib/simulation/package-info.java b/src/main/java/swervelib/simulation/package-info.java index 9aa4aece..908fc590 100644 --- a/src/main/java/swervelib/simulation/package-info.java +++ b/src/main/java/swervelib/simulation/package-info.java @@ -1,4 +1,2 @@ -/** - * Classes used to simulate the swerve drive. - */ +/** Classes used to simulate the swerve drive. */ package swervelib.simulation; diff --git a/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java b/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java index 7c1f5461..e9d98777 100644 --- a/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java +++ b/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java @@ -4,115 +4,80 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** - * Telemetry to describe the {@link swervelib.SwerveDrive} following frc-web-components. (Which follows AdvantageKit) + * Telemetry to describe the {@link swervelib.SwerveDrive} following frc-web-components. (Which + * follows AdvantageKit) */ -public class SwerveDriveTelemetry -{ +public class SwerveDriveTelemetry { - /** - * The current telemetry verbosity level. - */ - public static TelemetryVerbosity verbosity = TelemetryVerbosity.MACHINE; - /** - * State of simulation of the Robot, used to optimize retrieval. - */ - public static boolean isSimulation = RobotBase.isSimulation(); - /** - * The number of swerve modules - */ - public static int moduleCount; - /** - * The number of swerve modules - */ - public static double[] wheelLocations; - /** - * An array of rotation and velocity values describing the measured state of each swerve module - */ - public static double[] measuredStates; - /** - * An array of rotation and velocity values describing the desired state of each swerve module - */ - public static double[] desiredStates; - /** - * The robot's current rotation based on odometry or gyro readings - */ - public static double robotRotation = 0; - /** - * The maximum achievable speed of the modules, used to adjust the size of the vectors. - */ - public static double maxSpeed; - /** - * The units of the module rotations and robot rotation - */ - public static String rotationUnit = "degrees"; - /** - * The distance between the left and right modules. - */ - public static double sizeLeftRight; - /** - * The distance between the front and back modules. - */ - public static double sizeFrontBack; - /** - * The direction the robot should be facing when the "Robot Rotation" is zero or blank. This option is often useful to - * align with odometry data or match videos. 'up', 'right', 'down' or 'left' - */ - public static String forwardDirection = "up"; - /** - * The maximum achievable angular velocity of the robot. This is used to visualize the angular velocity from the - * chassis speeds properties. - */ - public static double maxAngularVelocity; - /** - * The maximum achievable angular velocity of the robot. This is used to visualize the angular velocity from the - * chassis speeds properties. - */ - public static double[] measuredChassisSpeeds = new double[3]; - /** - * Describes the desired forward, sideways and angular velocity of the robot. - */ - public static double[] desiredChassisSpeeds = new double[3]; + /** The current telemetry verbosity level. */ + public static TelemetryVerbosity verbosity = TelemetryVerbosity.MACHINE; + /** State of simulation of the Robot, used to optimize retrieval. */ + public static boolean isSimulation = RobotBase.isSimulation(); + /** The number of swerve modules */ + public static int moduleCount; + /** The number of swerve modules */ + public static double[] wheelLocations; + /** + * An array of rotation and velocity values describing the measured state of each swerve module + */ + public static double[] measuredStates; + /** An array of rotation and velocity values describing the desired state of each swerve module */ + public static double[] desiredStates; + /** The robot's current rotation based on odometry or gyro readings */ + public static double robotRotation = 0; + /** The maximum achievable speed of the modules, used to adjust the size of the vectors. */ + public static double maxSpeed; + /** The units of the module rotations and robot rotation */ + public static String rotationUnit = "degrees"; + /** The distance between the left and right modules. */ + public static double sizeLeftRight; + /** The distance between the front and back modules. */ + public static double sizeFrontBack; + /** + * The direction the robot should be facing when the "Robot Rotation" is zero or blank. This + * option is often useful to align with odometry data or match videos. 'up', 'right', 'down' or + * 'left' + */ + public static String forwardDirection = "up"; + /** + * The maximum achievable angular velocity of the robot. This is used to visualize the angular + * velocity from the chassis speeds properties. + */ + public static double maxAngularVelocity; + /** + * The maximum achievable angular velocity of the robot. This is used to visualize the angular + * velocity from the chassis speeds properties. + */ + public static double[] measuredChassisSpeeds = new double[3]; + /** Describes the desired forward, sideways and angular velocity of the robot. */ + public static double[] desiredChassisSpeeds = new double[3]; - /** - * Upload data to smartdashboard - */ - public static void updateData() - { - SmartDashboard.putNumber("swerve/moduleCount", moduleCount); - SmartDashboard.putNumberArray("swerve/wheelLocations", wheelLocations); - SmartDashboard.putNumberArray("swerve/measuredStates", measuredStates); - SmartDashboard.putNumberArray("swerve/desiredStates", desiredStates); - SmartDashboard.putNumber("swerve/robotRotation", robotRotation); - SmartDashboard.putNumber("swerve/maxSpeed", maxSpeed); - SmartDashboard.putString("swerve/rotationUnit", rotationUnit); - SmartDashboard.putNumber("swerve/sizeLeftRight", sizeLeftRight); - SmartDashboard.putNumber("swerve/sizeFrontBack", sizeFrontBack); - SmartDashboard.putString("swerve/forwardDirection", forwardDirection); - SmartDashboard.putNumber("swerve/maxAngularVelocity", maxAngularVelocity); - SmartDashboard.putNumberArray("swerve/measuredChassisSpeeds", measuredChassisSpeeds); - SmartDashboard.putNumberArray("swerve/desiredChassisSpeeds", desiredChassisSpeeds); - } + /** Upload data to smartdashboard */ + public static void updateData() { + SmartDashboard.putNumber("swerve/moduleCount", moduleCount); + SmartDashboard.putNumberArray("swerve/wheelLocations", wheelLocations); + SmartDashboard.putNumberArray("swerve/measuredStates", measuredStates); + SmartDashboard.putNumberArray("swerve/desiredStates", desiredStates); + SmartDashboard.putNumber("swerve/robotRotation", robotRotation); + SmartDashboard.putNumber("swerve/maxSpeed", maxSpeed); + SmartDashboard.putString("swerve/rotationUnit", rotationUnit); + SmartDashboard.putNumber("swerve/sizeLeftRight", sizeLeftRight); + SmartDashboard.putNumber("swerve/sizeFrontBack", sizeFrontBack); + SmartDashboard.putString("swerve/forwardDirection", forwardDirection); + SmartDashboard.putNumber("swerve/maxAngularVelocity", maxAngularVelocity); + SmartDashboard.putNumberArray("swerve/measuredChassisSpeeds", measuredChassisSpeeds); + SmartDashboard.putNumberArray("swerve/desiredChassisSpeeds", desiredChassisSpeeds); + } - /** - * Verbosity of telemetry data sent back. - */ - public enum TelemetryVerbosity - { - /** - * No telemetry data is sent back. - */ - NONE, - /** - * Low telemetry data, only post the robot position on the field. - */ - LOW, - /** - * Full swerve drive data is sent back in both human and machine readable forms. - */ - HIGH, - /** - * Only send the machine readable data related to swerve drive. - */ - MACHINE - } + /** Verbosity of telemetry data sent back. */ + public enum TelemetryVerbosity { + /** No telemetry data is sent back. */ + NONE, + /** Low telemetry data, only post the robot position on the field. */ + LOW, + /** Full swerve drive data is sent back in both human and machine readable forms. */ + HIGH, + /** Only send the machine readable data related to swerve drive. */ + MACHINE + } } diff --git a/src/main/java/swervelib/telemetry/package-info.java b/src/main/java/swervelib/telemetry/package-info.java index 4b8f6112..2f47093d 100644 --- a/src/main/java/swervelib/telemetry/package-info.java +++ b/src/main/java/swervelib/telemetry/package-info.java @@ -1,4 +1,2 @@ -/** - * Telemetry package for sending data to NT4 or SmartDashboard. - */ -package swervelib.telemetry; \ No newline at end of file +/** Telemetry package for sending data to NT4 or SmartDashboard. */ +package swervelib.telemetry;