From 2a174f6398bbc1266f897925a18a9be917964522 Mon Sep 17 00:00:00 2001 From: Rahuldeb5 Date: Sun, 7 Jul 2024 08:16:04 -0400 Subject: [PATCH 1/6] Added DriveController.java --- .../stuypulse/robot/constants/Settings.java | 3 + .../swerve/controllers/DriveController.java | 68 +++++++++++++++++++ 2 files changed, 71 insertions(+) create mode 100644 src/main/java/com/stuypulse/robot/subsystems/swerve/controllers/DriveController.java diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 4f5af44..401374e 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -119,6 +119,9 @@ public interface Swerve { double MAX_MODULE_SPEED = 4.9; double MAX_MODULE_ACCEL = 15.0; + double MAX_LINEAR_VELOCITY = 15.0; + double MAX_ANGULAR_VELOCITY = 12.0; + double MODULE_VELOCITY_DEADBAND = 0.05; SmartNumber ALIGN_OMEGA_DEADBAND = new SmartNumber("Swerve/Align Omega Deadband", 0.05); // TODO: make 0.25 and test diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/controllers/DriveController.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/controllers/DriveController.java new file mode 100644 index 0000000..c54c86c --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/controllers/DriveController.java @@ -0,0 +1,68 @@ +package com.stuypulse.robot.subsystems.swerve.controllers; + +import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.stuylib.network.SmartNumber; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +public class DriveController { + private static final SmartNumber controllerDeadband = + new SmartNumber("Controller/DriveController/Deadband", 0.1); + private static final SmartNumber maxAngularVelocityScalar = + new SmartNumber("Controller/DriveController/MaxAngularVelocityScalar", 0.65); + + + private double controllerX = 0; + private double controllerY = 0; + private double controllerOmega = 0; + + /** + * Accepts new drive input from joysticks. + * + * @param x Desired x velocity scalar, -1..1 + * @param y Desired y velocity scalar, -1..1 + * @param omega Desired omega velocity scalar, -1..1 + */ + public void acceptDriveInput(double x, double y, double omega) { + controllerX = x; + controllerY = y; + controllerOmega = omega; + } + + public ChassisSpeeds update() { + Translation2d linearVelocity = calcLinearVelocity(controllerX, controllerY); + double omega = MathUtil.applyDeadband(controllerOmega, controllerDeadband.get()); + omega = Math.copySign(omega * omega, omega); + + final double maxLinearVelocity = Swerve.MAX_LINEAR_VELOCITY; + final double maxAngularVelocity = + Swerve.MAX_ANGULAR_VELOCITY * (maxAngularVelocityScalar.get()); + + return new ChassisSpeeds( + linearVelocity.getX() * maxLinearVelocity, + linearVelocity.getY() * maxLinearVelocity, + omega * maxAngularVelocity); + } + + + public static Translation2d calcLinearVelocity(double x, double y) { + // Apply deadband + double linearMagnitude = MathUtil.applyDeadband(Math.hypot(x, y), controllerDeadband.get()); + Rotation2d linearDirection = new Rotation2d(x, y); + + // Square magnitude + linearMagnitude = linearMagnitude * linearMagnitude; + + // Calcaulate new linear velocity + Translation2d linearVelocity = + new Pose2d(new Translation2d(), linearDirection) + .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) + .getTranslation(); + return linearVelocity; + } +} From 8a1fab6fff79ee1d0385fed923795c960289175f Mon Sep 17 00:00:00 2001 From: Rahuldeb5 Date: Sun, 7 Jul 2024 08:16:04 -0400 Subject: [PATCH 2/6] Added DriveController.java --- .../stuypulse/robot/constants/Settings.java | 3 + .../swerve/controllers/DriveController.java | 68 +++++++++++++++++++ 2 files changed, 71 insertions(+) create mode 100644 src/main/java/com/stuypulse/robot/subsystems/swerve/controllers/DriveController.java diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 0437ce6..9423fb9 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -160,6 +160,9 @@ public interface Swerve { double MAX_MODULE_SPEED = 4.9; double MAX_MODULE_ACCEL = 15.0; + double MAX_LINEAR_VELOCITY = 15.0; + double MAX_ANGULAR_VELOCITY = 12.0; + double MODULE_VELOCITY_DEADBAND = 0.05; SmartNumber ALIGN_OMEGA_DEADBAND = new SmartNumber("Swerve/Align Omega Deadband", 0.05); // TODO: make 0.25 and test diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/controllers/DriveController.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/controllers/DriveController.java new file mode 100644 index 0000000..c54c86c --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/controllers/DriveController.java @@ -0,0 +1,68 @@ +package com.stuypulse.robot.subsystems.swerve.controllers; + +import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.stuylib.network.SmartNumber; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +public class DriveController { + private static final SmartNumber controllerDeadband = + new SmartNumber("Controller/DriveController/Deadband", 0.1); + private static final SmartNumber maxAngularVelocityScalar = + new SmartNumber("Controller/DriveController/MaxAngularVelocityScalar", 0.65); + + + private double controllerX = 0; + private double controllerY = 0; + private double controllerOmega = 0; + + /** + * Accepts new drive input from joysticks. + * + * @param x Desired x velocity scalar, -1..1 + * @param y Desired y velocity scalar, -1..1 + * @param omega Desired omega velocity scalar, -1..1 + */ + public void acceptDriveInput(double x, double y, double omega) { + controllerX = x; + controllerY = y; + controllerOmega = omega; + } + + public ChassisSpeeds update() { + Translation2d linearVelocity = calcLinearVelocity(controllerX, controllerY); + double omega = MathUtil.applyDeadband(controllerOmega, controllerDeadband.get()); + omega = Math.copySign(omega * omega, omega); + + final double maxLinearVelocity = Swerve.MAX_LINEAR_VELOCITY; + final double maxAngularVelocity = + Swerve.MAX_ANGULAR_VELOCITY * (maxAngularVelocityScalar.get()); + + return new ChassisSpeeds( + linearVelocity.getX() * maxLinearVelocity, + linearVelocity.getY() * maxLinearVelocity, + omega * maxAngularVelocity); + } + + + public static Translation2d calcLinearVelocity(double x, double y) { + // Apply deadband + double linearMagnitude = MathUtil.applyDeadband(Math.hypot(x, y), controllerDeadband.get()); + Rotation2d linearDirection = new Rotation2d(x, y); + + // Square magnitude + linearMagnitude = linearMagnitude * linearMagnitude; + + // Calcaulate new linear velocity + Translation2d linearVelocity = + new Pose2d(new Translation2d(), linearDirection) + .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) + .getTranslation(); + return linearVelocity; + } +} From 72ccc130a94b7e5819283ee0df61ec95d9dd920e Mon Sep 17 00:00:00 2001 From: Rahuldeb5 Date: Sun, 7 Jul 2024 20:16:52 -0400 Subject: [PATCH 3/6] Moving upstream to main --- .../robot/subsystems/swerve/modules/KrakenSwerveModule.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java index a38bd07..962e89a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java @@ -208,7 +208,7 @@ public void setDriveVelocity(double velocityRadsPerSec, double feedForward) { .withFeedForward(feedForward)); } - public void runTurnPositionSetpoint(double angleRads) { + public void setTurnPosition(double angleRads) { turnMotor.setControl(positionControl.withPosition(Units.radiansToRotations(angleRads))); } From fe2737b99cfed94b7bc61f41169192f09be32614 Mon Sep 17 00:00:00 2001 From: Rahuldeb5 Date: Sun, 7 Jul 2024 22:42:04 -0400 Subject: [PATCH 4/6] Updated constants and translation offsets --- .../stuypulse/robot/constants/Settings.java | 61 ++++++++++--------- .../robot/subsystems/swerve/SwerveDrive.java | 59 ++++-------------- .../swerve/modules/KrakenSwerveModule.java | 3 +- .../swerve/modules/SwerveModule.java | 10 +-- 4 files changed, 46 insertions(+), 87 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 9423fb9..229b62b 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -15,6 +15,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; /*- @@ -152,9 +153,8 @@ public interface FF { } public interface Swerve { - // between wheel centers - double WIDTH = Units.inchesToMeters(20.75); - double LENGTH = Units.inchesToMeters(20.75); + double WIDTH = Units.inchesToMeters(32.5); + double LENGTH = Units.inchesToMeters(27.375); double CENTER_TO_INTAKE_FRONT = Units.inchesToMeters(13.0); double MAX_MODULE_SPEED = 4.9; @@ -167,37 +167,37 @@ public interface Swerve { SmartNumber ALIGN_OMEGA_DEADBAND = new SmartNumber("Swerve/Align Omega Deadband", 0.05); // TODO: make 0.25 and test - public interface Assist { - SmartNumber ALIGN_MIN_SPEAKER_DIST = new SmartNumber("SwerveAssist/Minimum Distance to Speaker", 4); //change + // public interface Assist { + // SmartNumber ALIGN_MIN_SPEAKER_DIST = new SmartNumber("SwerveAssist/Minimum Distance to Speaker", 4); //change - double BUZZ_INTENSITY = 1; + // double BUZZ_INTENSITY = 1; - // angle PID - SmartNumber kP = new SmartNumber("SwerveAssist/kP", 6.0); - SmartNumber kI = new SmartNumber("SwerveAssist/kI", 0.0); - SmartNumber kD = new SmartNumber("SwerveAssist/kD", 0.0); + // // angle PID + // SmartNumber kP = new SmartNumber("SwerveAssist/kP", 6.0); + // SmartNumber kI = new SmartNumber("SwerveAssist/kI", 0.0); + // SmartNumber kD = new SmartNumber("SwerveAssist/kD", 0.0); - double ANGLE_DERIV_RC = 0.05; - double REDUCED_FF_DIST = 0.75; - } + // double ANGLE_DERIV_RC = 0.05; + // double REDUCED_FF_DIST = 0.75; + // } - // TODO: Tune these values - public interface Motion { - SmartNumber MAX_VELOCITY = new SmartNumber("Swerve/Motion/Max Velocity", 3.0); - SmartNumber MAX_ACCELERATION = new SmartNumber("Swerve/Motion/Max Acceleration", 4.0); - SmartNumber MAX_ANGULAR_VELOCITY = new SmartNumber("Swerve/Motion/Max Angular Velocity", Units.degreesToRadians(540)); - SmartNumber MAX_ANGULAR_ACCELERATION = new SmartNumber("Swerve/Motion/Max Angular Acceleration", Units.degreesToRadians(720)); - - PathConstraints DEFAULT_CONSTRAINTS = - new PathConstraints( - MAX_VELOCITY.get(), - MAX_ACCELERATION.get(), - MAX_ANGULAR_VELOCITY.get(), - MAX_ANGULAR_ACCELERATION.get()); - - PIDConstants XY = new PIDConstants(2.5, 0, 0.02); - PIDConstants THETA = new PIDConstants(4, 0, 0.1); - } + // // TODO: Tune these values + // public interface Motion { + // SmartNumber MAX_VELOCITY = new SmartNumber("Swerve/Motion/Max Velocity", 3.0); + // SmartNumber MAX_ACCELERATION = new SmartNumber("Swerve/Motion/Max Acceleration", 4.0); + // SmartNumber MAX_ANGULAR_VELOCITY = new SmartNumber("Swerve/Motion/Max Angular Velocity", Units.degreesToRadians(540)); + // SmartNumber MAX_ANGULAR_ACCELERATION = new SmartNumber("Swerve/Motion/Max Angular Acceleration", Units.degreesToRadians(720)); + + // PathConstraints DEFAULT_CONSTRAINTS = + // new PathConstraints( + // MAX_VELOCITY.get(), + // MAX_ACCELERATION.get(), + // MAX_ANGULAR_VELOCITY.get(), + // MAX_ANGULAR_ACCELERATION.get()); + + // PIDConstants XY = new PIDConstants(2.5, 0, 0.02); + // PIDConstants THETA = new PIDConstants(4, 0, 0.1); + // } public interface Encoder { public interface Drive { @@ -223,6 +223,7 @@ public interface Turn { SmartNumber kS = new SmartNumber("Swerve/Turn/FF/kS", 0.25582); SmartNumber kV = new SmartNumber("Swerve/Turn/FF/kV", 0.00205); SmartNumber kA = new SmartNumber("Swerve/Turn/FF/kA", 0.00020123); + double kT = 1.0 / DCMotor.getKrakenX60Foc(1).KtNMPerAmp; SmartBoolean INVERTED = new SmartBoolean("Swerve/Turn/INVERTED", true); diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index 77c78b3..8265acd 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -36,43 +36,6 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.hardware.Pigeon2; -/* - * Fields: - * - swerveModules: AbstractSwerveModule... - * - kinematics: SwerveDriveKinematics - * - gyro: AHRS - * - modules2D: FieldObject2D[] - * - * Tasks: - * - drive - * - followDrive - * - trackingDrive - * - aligning (Trap, Speaker, Amp) - * - GTADrive (shooting while driving) - * - * Methods: - * + singleton - * + initFieldObject(Field2D field): void - * + getModulePositions(): Translation2D[] - * + getModuleStates(): SwerveModuleStates[] - * + getModuleOffsets(): Rotation2D[] - * + getChassisSpeeds(): ChassisSpeed[] - * + setModuleStates(SwerveModuleState... states): void - * + setChassisSpeed(ChassisSpeed): void - * + drive(double, Rotation2D) - * + stop(double, Rotation2D) - * - * SwerveDrive.java - * Methods: - * - getGyroAngle(): Rotation2D - * - getGyroYaw(): Rotation2D - * - getGyroPitch(): Rotation2D - * - getGyroRoll(): Rotation2D - * - getKinematics(): SwerveDriveKinematics - * + periodic(): void - * - * - */ public class SwerveDrive extends SubsystemBase { private static final SwerveDrive instance; @@ -80,10 +43,10 @@ public class SwerveDrive extends SubsystemBase { static { if (Robot.ROBOT == RobotType.SELF_REINFORCED_VELVEETA_CHEESE_POLYPROPYLENE_GOOBER) { instance = new SwerveDrive( - new KrakenSwerveModule(FrontRight.ID, FrontRight.MODULE_OFFSET, FrontRight.ABSOLUTE_OFFSET, Ports.Swerve.FrontRight.DRIVE, Ports.Swerve.FrontRight.TURN, Ports.Swerve.FrontRight.ENCODER), - new KrakenSwerveModule(FrontLeft.ID, FrontLeft.MODULE_OFFSET, FrontLeft.ABSOLUTE_OFFSET, Ports.Swerve.FrontLeft.DRIVE, Ports.Swerve.FrontLeft.TURN, Ports.Swerve.FrontLeft.ENCODER), - new KrakenSwerveModule(BackLeft.ID, BackLeft.MODULE_OFFSET, BackLeft.ABSOLUTE_OFFSET, Ports.Swerve.BackLeft.DRIVE, Ports.Swerve.BackLeft.TURN, Ports.Swerve.BackLeft.ENCODER), - new KrakenSwerveModule(BackRight.ID, BackRight.MODULE_OFFSET, BackRight.ABSOLUTE_OFFSET, Ports.Swerve.BackRight.DRIVE, Ports.Swerve.BackRight.TURN, Ports.Swerve.BackRight.ENCODER) + new KrakenSwerveModule(FrontRight.ID, FrontRight.ABSOLUTE_OFFSET, Ports.Swerve.FrontRight.DRIVE, Ports.Swerve.FrontRight.TURN, Ports.Swerve.FrontRight.ENCODER), + new KrakenSwerveModule(FrontLeft.ID, FrontLeft.ABSOLUTE_OFFSET, Ports.Swerve.FrontLeft.DRIVE, Ports.Swerve.FrontLeft.TURN, Ports.Swerve.FrontLeft.ENCODER), + new KrakenSwerveModule(BackLeft.ID, BackLeft.ABSOLUTE_OFFSET, Ports.Swerve.BackLeft.DRIVE, Ports.Swerve.BackLeft.TURN, Ports.Swerve.BackLeft.ENCODER), + new KrakenSwerveModule(BackRight.ID, BackRight.ABSOLUTE_OFFSET, Ports.Swerve.BackRight.DRIVE, Ports.Swerve.BackRight.TURN, Ports.Swerve.BackRight.ENCODER) ); } else { instance = null; // TODO: Change this to something. Just !nothing. @@ -109,7 +72,7 @@ public static SwerveDrive getInstance() { protected SwerveDrive(SwerveModule... modules) { this.modules = modules; kinematics = new SwerveDriveKinematics(getModuleOffsets()); - gyro = new Pigeon2(Ports.Gyro.PIGEON2); + gyro = new Pigeon2(Ports.Gyro.PIGEON2, "*"); modules2D = new FieldObject2d[modules.length]; statesPub = NetworkTableInstance.getDefault() @@ -143,12 +106,14 @@ public SwerveModulePosition[] getModulePositions() { return offsets; } + // TODO: Offset should be to the center of wheels, not corner of robot public Translation2d[] getModuleOffsets() { - Translation2d[] offsets = new Translation2d[modules.length]; - for (int i = 0; i < modules.length; i++) { - offsets[i] = modules[i].getModuleOffset(); - } - return offsets; + return new Translation2d[] { + new Translation2d(Swerve.WIDTH / 2.0, Swerve.LENGTH / 2.0), + new Translation2d(-Swerve.WIDTH / 2.0, Swerve.LENGTH / 2.0), + new Translation2d(-Swerve.WIDTH / 2.0, -Swerve.LENGTH / 2.0), + new Translation2d(Swerve.WIDTH / 2.0, -Swerve.LENGTH / 2.0) + }; } public ChassisSpeeds getChassisSpeeds() { diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java index 962e89a..fb5000f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java @@ -73,13 +73,12 @@ public class KrakenSwerveModule extends SwerveModule { public KrakenSwerveModule( String id, - Translation2d location, Rotation2d angleOffset, int driveMotorID, int turnMotorID, int turnEncoderID ) { - super(id, location); + super(id); this.angleOffset = angleOffset; diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SwerveModule.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SwerveModule.java index 234f347..3811717 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SwerveModule.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SwerveModule.java @@ -15,14 +15,12 @@ public abstract class SwerveModule extends SubsystemBase { private final String id; - private final Translation2d offset; private SwerveModuleState targetState; - public SwerveModule(String id, Translation2d offset) { + public SwerveModule(String id) { this.id = id; - this.offset = offset; - + targetState = new SwerveModuleState(); } @@ -30,10 +28,6 @@ public final String getId() { return this.id; } - public final Translation2d getModuleOffset() { - return this.offset; - } - public abstract double getVelocity(); public abstract Rotation2d getAngle(); From 6913608a36e2ecc5699941dd2d49a7594dccd195 Mon Sep 17 00:00:00 2001 From: Rahuldeb5 Date: Mon, 8 Jul 2024 12:48:10 -0400 Subject: [PATCH 5/6] Added StatusSignals. TODO: Create functions for characterizations and setpoints --- .../robot/subsystems/swerve/SwerveDrive.java | 26 ++++- .../swerve/modules/KrakenSwerveModule.java | 97 ++++++++++++++++--- .../swerve/modules/SwerveModule.java | 1 - 3 files changed, 107 insertions(+), 17 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index 8265acd..0295916 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -33,7 +33,9 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.hardware.Pigeon2; public class SwerveDrive extends SubsystemBase { @@ -64,6 +66,10 @@ public static SwerveDrive getInstance() { private final StructArrayPublisher statesPub; + // Status Signals + private final StatusSignal yaw; + private final StatusSignal yawVelocity; + /** * Creates a new Swerve Drive using the provided modules * @@ -72,11 +78,19 @@ public static SwerveDrive getInstance() { protected SwerveDrive(SwerveModule... modules) { this.modules = modules; kinematics = new SwerveDriveKinematics(getModuleOffsets()); - gyro = new Pigeon2(Ports.Gyro.PIGEON2, "*"); modules2D = new FieldObject2d[modules.length]; + gyro = new Pigeon2(Ports.Gyro.PIGEON2, "*"); statesPub = NetworkTableInstance.getDefault() .getStructArrayTopic("SmartDashboard/Swerve/States", SwerveModuleState.struct).publish(); + + yaw = gyro.getYaw(); + yawVelocity = gyro.getAngularVelocityZWorld(); + gyro.getConfigurator().apply(new Pigeon2Configuration()); + gyro.getConfigurator().setYaw(0.0); + yaw.setUpdateFrequency(250); + yawVelocity.setUpdateFrequency(100); + gyro.optimizeBusUtilization(); } public void initFieldObject(Field2d field) { @@ -188,20 +202,22 @@ public Rotation2d getGyroAngle() { } public StatusSignal getGyroYaw() { - return gyro.getYaw(); + return yaw; } public StatusSignal getGyroYawVelocity() { - return gyro.getAngularVelocityZWorld(); + return yawVelocity; } @Override public void periodic() { statesPub.set(getModuleStates()); + BaseStatusSignal.refreshAll(yaw, yawVelocity); + SmartDashboard.putNumber("Swerve/Gyro/Angle (deg)", getGyroAngle().getDegrees()); - SmartDashboard.putNumber("Swerve/Gyro/Yaw (deg)", getGyroYaw().getValueAsDouble()); - SmartDashboard.putNumber("Swerve/Gyro/Yaw Velocity (deg)", getGyroYawVelocity().getValueAsDouble()); + SmartDashboard.putNumber("Swerve/Gyro/Yaw (deg)", yaw.getValueAsDouble()); + SmartDashboard.putNumber("Swerve/Gyro/Yaw Velocity (deg)", yawVelocity.getValueAsDouble()); SmartDashboard.putNumber("Swerve/X Acceleration (Gs)", gyro.getAccelerationX().getValueAsDouble()); SmartDashboard.putNumber("Swerve/Y Acceleration (Gs)", gyro.getAccelerationY().getValueAsDouble()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java index fb5000f..4f1aaa1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java @@ -7,7 +7,10 @@ import java.util.concurrent.Executor; import java.util.concurrent.Executors; +import java.util.function.Supplier; +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; @@ -15,7 +18,6 @@ import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -32,9 +34,10 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class KrakenSwerveModule extends SwerveModule { @@ -52,12 +55,26 @@ public class KrakenSwerveModule extends SwerveModule { private final TalonFX driveMotor; private final TalonFX turnMotor; - private final CANcoder turnEncoder; + private final AnalogInput turnEncoder; private final AngleController pivotController; private final IFilter targetAcceleration; + // Status Signals + private final StatusSignal drivePosition; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveSupplyCurrent; + private final StatusSignal driveTorqueCurrent; + + private final StatusSignal turnPosition; + private final Supplier turnAbsolutePosition; + private final StatusSignal turnVelocity; + private final StatusSignal turnAppliedVolts; + private final StatusSignal turnSupplyCurrent; + private final StatusSignal turnTorqueCurrent; + private final TalonFXConfiguration driveConfig = new TalonFXConfiguration(); private final TalonFXConfiguration turnConfig = new TalonFXConfiguration(); private static final Executor brakeModeExecutor = Executors.newFixedThreadPool(8); // TODO: Choose n threaqds @@ -84,7 +101,7 @@ public KrakenSwerveModule( driveMotor = new TalonFX(driveMotorID, "*"); turnMotor = new TalonFX(turnMotorID, "*"); - turnEncoder = new CANcoder(turnEncoderID); + turnEncoder = new AnalogInput(turnEncoderID); setDrivePID(Swerve.Drive.kP.doubleValue(), Swerve.Drive.kI.doubleValue(), Swerve.Drive.kD.doubleValue()); setTurnPID(Swerve.Turn.kP.doubleValue(), Swerve.Turn.kI.doubleValue(), Swerve.Turn.kD.doubleValue()); @@ -120,7 +137,40 @@ public KrakenSwerveModule( // driveConfig.CurrentLimits.StatorCurrentLimit = 65; // 65A stator current limit // driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; // Enable stator current limiting - driveMotor.setPosition(0); + // 250 hz signals + drivePosition = driveMotor.getPosition(); + turnPosition = turnMotor.getPosition(); + BaseStatusSignal.setUpdateFrequencyForAll(250, drivePosition, turnPosition); + + // 100 hz signals + driveVelocity = driveMotor.getVelocity(); + driveAppliedVolts = driveMotor.getMotorVoltage(); + driveSupplyCurrent = driveMotor.getSupplyCurrent(); + driveTorqueCurrent = driveMotor.getTorqueCurrent(); + turnAbsolutePosition = + () -> + Rotation2d.fromRadians( + turnEncoder.getVoltage() + / RobotController.getVoltage5V() + * (2 * Math.PI)) + .minus(angleOffset); + turnVelocity = turnMotor.getVelocity(); + turnAppliedVolts = turnMotor.getMotorVoltage(); + turnSupplyCurrent = turnMotor.getSupplyCurrent(); + turnTorqueCurrent = turnMotor.getTorqueCurrent(); + BaseStatusSignal.setUpdateFrequencyForAll( + 100, + driveVelocity, + driveAppliedVolts, + driveSupplyCurrent, + driveTorqueCurrent, + turnVelocity, + turnAppliedVolts, + turnSupplyCurrent, + turnTorqueCurrent); + + // driveMotor.setPosition(0); + turnMotor.setPosition(turnAbsolutePosition.get().getRotations(), 1.0); pivotController = new AnglePIDController(Turn.kP, Turn.kI, Turn.kD) .setOutputFilter(x -> -x); @@ -143,25 +193,25 @@ public KrakenSwerveModule( /****************/ public double getPosition() { - return driveMotor.getPosition().getValueAsDouble() * Encoder.Drive.POSITION_CONVERSION; + return drivePosition.getValueAsDouble() * Encoder.Drive.POSITION_CONVERSION; } public double getTurnPosition() { - return turnMotor.getPosition().getValueAsDouble() * Encoder.Turn.POSITION_CONVERSION; + return turnPosition.getValueAsDouble() * Encoder.Turn.POSITION_CONVERSION; } @Override public double getVelocity() { - return driveMotor.getVelocity().getValueAsDouble() * Encoder.Drive.POSITION_CONVERSION; + return driveVelocity.getValueAsDouble() * Encoder.Drive.POSITION_CONVERSION; } public double getTurnVelocity() { - return turnMotor.getVelocity().getValueAsDouble() * Encoder.Turn.POSITION_CONVERSION; + return turnVelocity.getValueAsDouble() * Encoder.Turn.POSITION_CONVERSION; } @Override public Rotation2d getAngle() { - return Rotation2d.fromRotations(turnEncoder.getAbsolutePosition().getValueAsDouble()) + return Rotation2d.fromRotations(turnAbsolutePosition.get().getRotations()) .minus(angleOffset); } @@ -200,6 +250,11 @@ public void setCharacterization(double input) { driveMotor.setControl(currentControl.withOutput(input)); } + public void setCharacterization(double turnSetpointRads, double input) { + setTurnPosition(turnSetpointRads); + setCharacterization(input); + } + public void setDriveVelocity(double velocityRadsPerSec, double feedForward) { driveMotor.setControl( velocityTorqueCurrentFOC @@ -233,6 +288,11 @@ public void setTurnBrakeMode(boolean enable) { }); } + public void setBrakeMode(boolean enable) { + setDriveBrakeMode(enable); + setTurnBrakeMode(enable); + } + public void stop() { driveMotor.setControl(neutralControl); turnMotor.setControl(neutralControl); @@ -242,10 +302,25 @@ private double convertDriveVel(double speedMetersPerSecond) { return speedMetersPerSecond / Encoder.Drive.POSITION_CONVERSION; } + // TODO: FIX PERIODIC @Override public void periodic() { super.periodic(); + BaseStatusSignal.refreshAll( + drivePosition, + driveVelocity, + driveAppliedVolts, + driveSupplyCurrent, + driveTorqueCurrent); + + BaseStatusSignal.refreshAll( + turnPosition, + turnVelocity, + turnAppliedVolts, + turnSupplyCurrent, + turnTorqueCurrent); + final boolean USE_ACCEL = true; double velocity = convertDriveVel(getTargetState().speedMetersPerSecond); @@ -278,7 +353,7 @@ public void periodic() { SmartDashboard.putNumber("Swerve/Modules/" + getId() + "/Turn Voltage", pivotController.getOutput()); SmartDashboard.putNumber("Swerve/Modules/" + getId() + "/Turn Current", turnMotor.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("Swerve/Modules/" + getId() + "/Angle Error", pivotController.getError().toDegrees()); - SmartDashboard.putNumber("Swerve/Modules/" + getId() + "/Raw Encoder Angle", Units.rotationsToDegrees(turnEncoder.getAbsolutePosition().getValueAsDouble())); + SmartDashboard.putNumber("Swerve/Modules/" + getId() + "/Raw Encoder Angle", Units.rotationsToDegrees(turnAbsolutePosition.get().getRotations())); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SwerveModule.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SwerveModule.java index 3811717..1d41c89 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SwerveModule.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SwerveModule.java @@ -6,7 +6,6 @@ package com.stuypulse.robot.subsystems.swerve.modules; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; From 2ca5efaa7358c125287f74006a69bef484d6fc3c Mon Sep 17 00:00:00 2001 From: Rahuldeb5 Date: Wed, 10 Jul 2024 09:51:15 -0400 Subject: [PATCH 6/6] Commiting to rebase branch --- .../com/stuypulse/robot/constants/Settings.java | 3 --- .../robot/subsystems/swerve/SwerveDrive.java | 7 +++++++ .../swerve/modules/KrakenSwerveModule.java | 14 +++++--------- 3 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 229b62b..0a5c314 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -5,9 +5,6 @@ package com.stuypulse.robot.constants; -import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.util.PIDConstants; - import com.stuypulse.robot.util.ShooterSpeeds; import com.stuypulse.stuylib.network.SmartBoolean; diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index 0295916..9486004 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -15,6 +15,7 @@ import com.stuypulse.robot.constants.Settings.Swerve.BackRight; import com.stuypulse.robot.constants.Settings.Swerve.FrontLeft; import com.stuypulse.robot.constants.Settings.Swerve.FrontRight; +import com.stuypulse.robot.subsystems.swerve.controllers.DriveController; import com.stuypulse.robot.subsystems.swerve.modules.KrakenSwerveModule; import com.stuypulse.robot.subsystems.swerve.modules.SwerveModule; @@ -64,6 +65,10 @@ public static SwerveDrive getInstance() { private final Pigeon2 gyro; private final FieldObject2d[] modules2D; + private ChassisSpeeds desiredSpeeds = new ChassisSpeeds(); + // TODO: Add SSG + private final DriveController driveController; + private final StructArrayPublisher statesPub; // Status Signals @@ -81,6 +86,8 @@ protected SwerveDrive(SwerveModule... modules) { modules2D = new FieldObject2d[modules.length]; gyro = new Pigeon2(Ports.Gyro.PIGEON2, "*"); + driveController = new DriveController(); + statesPub = NetworkTableInstance.getDefault() .getStructArrayTopic("SmartDashboard/Swerve/States", SwerveModuleState.struct).publish(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java index 4f1aaa1..01c4c02 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java @@ -18,6 +18,7 @@ import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -36,8 +37,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.AnalogInput; -import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class KrakenSwerveModule extends SwerveModule { @@ -55,7 +54,7 @@ public class KrakenSwerveModule extends SwerveModule { private final TalonFX driveMotor; private final TalonFX turnMotor; - private final AnalogInput turnEncoder; + private final CANcoder turnEncoder; private final AngleController pivotController; @@ -101,7 +100,7 @@ public KrakenSwerveModule( driveMotor = new TalonFX(driveMotorID, "*"); turnMotor = new TalonFX(turnMotorID, "*"); - turnEncoder = new AnalogInput(turnEncoderID); + turnEncoder = new CANcoder(turnEncoderID, "*"); setDrivePID(Swerve.Drive.kP.doubleValue(), Swerve.Drive.kI.doubleValue(), Swerve.Drive.kD.doubleValue()); setTurnPID(Swerve.Turn.kP.doubleValue(), Swerve.Turn.kI.doubleValue(), Swerve.Turn.kD.doubleValue()); @@ -149,10 +148,7 @@ public KrakenSwerveModule( driveTorqueCurrent = driveMotor.getTorqueCurrent(); turnAbsolutePosition = () -> - Rotation2d.fromRadians( - turnEncoder.getVoltage() - / RobotController.getVoltage5V() - * (2 * Math.PI)) + Rotation2d.fromRotations(turnEncoder.getAbsolutePosition().getValueAsDouble()) .minus(angleOffset); turnVelocity = turnMotor.getVelocity(); turnAppliedVolts = turnMotor.getMotorVoltage(); @@ -183,7 +179,7 @@ public KrakenSwerveModule( driveMotor.optimizeBusUtilization(); turnMotor.optimizeBusUtilization(); - // turnMotor is a TalonFX, so we need to configure it as such + // turnMotor is a TalonFX, so we do not need to configure it as such // Motors.disableStatusFrames(turnMotor, StatusFrame.ANALOG_SENSOR, StatusFrame.ALTERNATE_ENCODER, StatusFrame.ABS_ENCODER_POSIITION, StatusFrame.ABS_ENCODER_VELOCITY); // Motors.Swerve.TURN_CONFIG.configure(turnMotor); }