From 8633032f053d7bb8c98cb747b20e851989ce25e1 Mon Sep 17 00:00:00 2001 From: Max M Date: Mon, 5 Feb 2024 20:28:17 -0600 Subject: [PATCH] update driving code --- .wpilib/wpilib_preferences.json | 2 +- .../deploy/pathplanner/autos/New Auto.auto | 8 +- .../pathplanner/paths/Example Path.path | 49 ------ src/main/deploy/pathplanner/paths/Path1.path | 161 ++++++++++++++++++ .../swervelib/Mk4iSwerveModuleHelper.java | 33 +++- .../swervelib/ctre/CtreUtils.java | 8 + .../KrakenDriveControllerFactoryBuilder.java | 91 ++++++++++ src/main/java/frc/robot/Constants.java | 7 +- .../java/frc/robot/subsystems/Drivetrain.java | 2 +- 9 files changed, 298 insertions(+), 63 deletions(-) delete mode 100644 src/main/deploy/pathplanner/paths/Example Path.path create mode 100644 src/main/deploy/pathplanner/paths/Path1.path create mode 100644 src/main/java/com/swervedrivespecialties/swervelib/ctre/KrakenDriveControllerFactoryBuilder.java diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 320d0b9..7770555 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -2,5 +2,5 @@ "enableCppIntellisense": false, "currentLanguage": "java", "projectYear": "2024", - "teamNumber": 3081 + "teamNumber": 2470 } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto index 70e50dd..567a7a8 100644 --- a/src/main/deploy/pathplanner/autos/New Auto.auto +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 2.0, - "y": 7.0 + "x": 0.7, + "y": 6.7 }, - "rotation": -1.0 + "rotation": 60.0 }, "command": { "type": "sequential", @@ -14,7 +14,7 @@ { "type": "path", "data": { - "pathName": "Example Path" + "pathName": "Path1" } } ] diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path deleted file mode 100644 index bbdcec0..0000000 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.0, - "y": 7.0 - }, - "prevControl": null, - "nextControl": { - "x": 2.828145140888627, - "y": 6.985544672795801 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.64241490982982, - "y": 7.0 - }, - "prevControl": { - "x": 3.12754000508001, - "y": 6.99536151415533 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Path1.path b/src/main/deploy/pathplanner/paths/Path1.path new file mode 100644 index 0000000..a2c51db --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Path1.path @@ -0,0 +1,161 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7, + "y": 6.7 + }, + "prevControl": null, + "nextControl": { + "x": 1.7164034718694439, + "y": 7.019690992506985 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.9518202708280417, + "y": 7.0 + }, + "prevControl": { + "x": 1.6549716576762825, + "y": 7.0 + }, + "nextControl": { + "x": 2.1717038325554348, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.766614212915926, + "y": 7.0 + }, + "prevControl": { + "x": 2.6061789698808355, + "y": 7.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0, + "rotationDegrees": 60.0, + "rotateFast": false + }, + { + "waypointRelativePos": 2.0, + "rotationDegrees": 0.0, + "rotateFast": false + }, + { + "waypointRelativePos": 1, + "rotationDegrees": 0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "first shot", + "waypointRelativePos": 1.3, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "stop" + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + } + ] + } + } + }, + { + "name": "pickup", + "waypointRelativePos": 1.55, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "stop" + } + }, + { + "type": "named", + "data": { + "name": "pickup" + } + } + ] + } + } + }, + { + "name": "second shot", + "waypointRelativePos": 2.0, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "stop" + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + } + ] + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 60.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/com/swervedrivespecialties/swervelib/Mk4iSwerveModuleHelper.java b/src/main/java/com/swervedrivespecialties/swervelib/Mk4iSwerveModuleHelper.java index 341f76f..e47d4b1 100644 --- a/src/main/java/com/swervedrivespecialties/swervelib/Mk4iSwerveModuleHelper.java +++ b/src/main/java/com/swervedrivespecialties/swervelib/Mk4iSwerveModuleHelper.java @@ -4,10 +4,7 @@ import com.swervedrivespecialties.swervelib.rev.*; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; -/** - * @deprecated use {@link MkSwerveModuleBuilder} instead, which provides support for CANivores. - */ -@Deprecated(since = "2023.1.2.0", forRemoval = true) + public final class Mk4iSwerveModuleHelper { private Mk4iSwerveModuleHelper() { } @@ -18,6 +15,12 @@ private static DriveControllerFactory getFalcon500DriveFactory(Mk4Mo .withCurrentLimit(configuration.getDriveCurrentLimit()) .build(); } + private static DriveControllerFactory getKrakenDriveFactory(Mk4ModuleConfiguration configuration) { + return new KrakenDriveControllerFactoryBuilder() + .withVoltageCompensation(configuration.getNominalVoltage()) + .withCurrentLimit(configuration.getDriveCurrentLimit()) + .build(); + } private static SteerControllerFactory> getFalcon500SteerFactory(Mk4ModuleConfiguration configuration) { return new Falcon500SteerControllerFactoryBuilder() @@ -193,6 +196,28 @@ public static SwerveModule createNeo( ); } + public static SwerveModule createKrakenNeo( + ShuffleboardLayout container, + Mk4ModuleConfiguration configuration, + GearRatio gearRatio, + int driveMotorPort, + int steerMotorPort, + int steerEncoderPort, + double steerOffset + ) { + return new SwerveModuleFactory<>( + gearRatio.getConfiguration(), + getKrakenDriveFactory(configuration), + getNeoSteerFactory(configuration) + ).create( + container, + driveMotorPort, + new SteerConfiguration<>( + steerMotorPort, + new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + ) + ); + } /** * Creates a Mk4i swerve module that uses NEOs for driving and steering. * Module information is displayed in the specified ShuffleBoard container. diff --git a/src/main/java/com/swervedrivespecialties/swervelib/ctre/CtreUtils.java b/src/main/java/com/swervedrivespecialties/swervelib/ctre/CtreUtils.java index f427c41..77d5c2a 100644 --- a/src/main/java/com/swervedrivespecialties/swervelib/ctre/CtreUtils.java +++ b/src/main/java/com/swervedrivespecialties/swervelib/ctre/CtreUtils.java @@ -1,12 +1,20 @@ package com.swervedrivespecialties.swervelib.ctre; import com.ctre.phoenix.ErrorCode; +import com.ctre.phoenix6.StatusCode; + import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotBase; public final class CtreUtils { private CtreUtils() {} + public static void checkCtreError(StatusCode statusCode, String message) { + if (RobotBase.isReal() && statusCode != StatusCode.OK) { + DriverStation.reportError(String.format("%s: %s", message, statusCode.toString()), false); + } + } + public static void checkCtreError(ErrorCode errorCode, String message) { if (RobotBase.isReal() && errorCode != ErrorCode.OK) { DriverStation.reportError(String.format("%s: %s", message, errorCode.toString()), false); diff --git a/src/main/java/com/swervedrivespecialties/swervelib/ctre/KrakenDriveControllerFactoryBuilder.java b/src/main/java/com/swervedrivespecialties/swervelib/ctre/KrakenDriveControllerFactoryBuilder.java new file mode 100644 index 0000000..1d47883 --- /dev/null +++ b/src/main/java/com/swervedrivespecialties/swervelib/ctre/KrakenDriveControllerFactoryBuilder.java @@ -0,0 +1,91 @@ +package com.swervedrivespecialties.swervelib.ctre; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.swervedrivespecialties.swervelib.DriveController; +import com.swervedrivespecialties.swervelib.DriveControllerFactory; +import com.swervedrivespecialties.swervelib.MechanicalConfiguration; + + +public class KrakenDriveControllerFactoryBuilder { + private double nominalVoltage = Double.NaN; + private double currentLimit = Double.NaN; + + public KrakenDriveControllerFactoryBuilder withVoltageCompensation(double minimalVoltage) { + this.nominalVoltage = minimalVoltage; + return this; + } + public KrakenDriveControllerFactoryBuilder withCurrentLimit(double limit) { + this.currentLimit = limit; + return this; + } + + public boolean hasVoltageCompensation() { + return Double.isFinite(nominalVoltage); + } + + public DriveControllerFactory build() { + return new FactoryImplementation(); + } + + public boolean hasCurrentLimit() { + return Double.isFinite(currentLimit); + } + + + + private class FactoryImplementation implements DriveControllerFactory { + @Override public ControllerImplementation create(Integer driveConfiguration, String canbus, MechanicalConfiguration mechConfiguration) { + TalonFXConfiguration motorConfig = new TalonFXConfiguration(); + motorConfig.MotorOutput.Inverted = mechConfiguration.isDriveInverted() ? + InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; + + double sensorPositionCoefficient = Math.PI * mechConfiguration.getWheelDiameter() * mechConfiguration.getDriveReduction(); + + if (hasCurrentLimit()) { + motorConfig.CurrentLimits.SupplyCurrentLimit = currentLimit; + motorConfig.CurrentLimits.StatorCurrentLimitEnable = true; + } + + if (hasVoltageCompensation()) { + motorConfig.Voltage.PeakForwardVoltage = nominalVoltage; + motorConfig.Voltage.PeakReverseVoltage = -nominalVoltage; + } + + TalonFX motor = new TalonFX(driveConfiguration); + + motor.setNeutralMode(NeutralModeValue.Brake); + + CtreUtils.checkCtreError(motor.getConfigurator().apply(motorConfig), "Failed to configure TalonFX"); + return new ControllerImplementation(motor, sensorPositionCoefficient); + } + } + + private class ControllerImplementation implements DriveController { + private TalonFX motor; + private final double sensorPositionCoefficient; + + public ControllerImplementation(TalonFX motor, double sensorPositionCoefficient) { + this.sensorPositionCoefficient = sensorPositionCoefficient; + this.motor = motor; + } + + @Override public Object getDriveMotor() { + return this.motor; + } + + @Override public void setReferenceVoltage(double voltage) { + this.motor.setVoltage(voltage); + } + + @Override public double getStateVelocity() { + return this.motor.getRotorVelocity().getValue() * sensorPositionCoefficient; + } + + @Override public double getStateDistance() { + return this.motor.getRotorPosition().getValue() * sensorPositionCoefficient; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3a8a5c5..d60e44a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -33,14 +33,13 @@ public enum CanBus { private CanBus(String value) { this.bus_name = value; } - } - + } public static class DriveConstants { // : Physical constants (motors, sensors, ...) public static final double kDriveVoltageCompensation = 12; public static final double kWheelDiameterMeters = Units.inchesToMeters(4); - public static final double kWheelBaseLengthMeters = Units.inchesToMeters(25.5); - public static final double kTrackWidthMeters = Units.inchesToMeters(20.5); + public static final double kWheelBaseLengthMeters = Units.inchesToMeters(11.375*2); + public static final double kTrackWidthMeters = Units.inchesToMeters(12.375*2); public static final double kDriveGearReduction = SdsModuleConfigurations.MK4I_L3.getDriveReduction(); diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 8ee1268..ad8e622 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -123,7 +123,7 @@ public Drivetrain(VisionSubsystem vision) { private SwerveModule createModule( ModuleConfig config, Mk4ModuleConfiguration moduleConfig, ShuffleboardTab tab) { - return Mk4iSwerveModuleHelper.createNeo( + return Mk4iSwerveModuleHelper.createKrakenNeo( tab.getLayout(config.name, BuiltInLayouts.kList) .withSize(2, 6) .withPosition(config.col, config.line),