From 75c632e463768348ef600458b125d466fa0fed49 Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Mon, 1 Apr 2024 13:17:09 -0400 Subject: [PATCH 1/2] [#516] Added Closed loop request, need to test --- src/main/java/frc/robot/Constants.java | 7 +++--- .../java/frc/robot/subsystems/Swerve.java | 23 +++++++++++-------- 2 files changed, 17 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3bf21c1d..f01dac32 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -16,6 +16,7 @@ import com.ctre.phoenix6.signals.SensorDirectionValue; import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.util.PIDConstants; +import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; @@ -159,11 +160,11 @@ public static class AutonomousConstants { public static final double MAX_MODULE_VELOCITY = Units.feetToMeters(16); // f/s to m/s public static final double DRIVE_BASE_RADIUS = Units.inchesToMeters(10.825); - public static final double CONTROL_LOOP_PERIOD = 0.01; // constants + public static final double CONTROL_LOOP_PERIOD = 0.02; // constants + public static final ReplanningConfig REPLANNING_CONFIG = new ReplanningConfig(true, false); // TODO Should we enable dynamic replaning public static final PathConstraints PATHFINDING_CONSTRAINTS = new PathConstraints(2.0, 1.0, 3.0, 1.5); - public static final PathConstraints PATH_CONSTRAINTS = new PathConstraints(2.0, 1, 1.0, 0.5); // TODO get - // constants + public static final PathConstraints PATH_CONSTRAINTS = new PathConstraints(2.0, 1, 1.0, 0.5); // TODO get constants public static final double BLUE_CHASE_BOUNDARY = 8.5; // The highest X value the robot can // be at before ending. diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index e81ca63c..f4008d77 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -8,6 +8,8 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.SteerRequestType; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.PathPlannerAuto; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; @@ -183,7 +185,7 @@ public Command applyPercentRequestField(DoubleSupplier x, DoubleSupplier y, DoubleSupplier rot) { return run(() -> this.setControl(driveField.withVelocityX(x.getAsDouble() * maxSpeed) .withVelocityY(y.getAsDouble() * maxSpeed) - .withRotationalRate(rot.getAsDouble() * maxAngularRate))); + .withRotationalRate(rot.getAsDouble() * maxAngularRate).withDriveRequestType(DriveRequestType.Velocity))); } /** @@ -194,7 +196,7 @@ public Command applyPercentRequestField(DoubleSupplier x, DoubleSupplier y, * @param rot the rotational velocity in rad/s */ public void setField(double x, double y, double rot) { - this.setControl(driveField.withVelocityX(x).withVelocityY(y).withRotationalRate(rot)); + this.setControl(driveField.withVelocityX(x).withVelocityY(y).withRotationalRate(rot).withDriveRequestType(DriveRequestType.Velocity)); } /** @@ -208,7 +210,7 @@ public void setField(double x, double y, double rot) { */ public void setFieldDriver(double x, double y, double rot) { this.setControl(driveField.withVelocityX(x * maxSpeed).withVelocityY(y * maxSpeed) - .withRotationalRate(rot)); + .withRotationalRate(rot).withDriveRequestType(DriveRequestType.Velocity)); } /** @@ -223,7 +225,7 @@ public Command applyPercentRequestRobot(DoubleSupplier x, DoubleSupplier y, DoubleSupplier rot) { return run(() -> this.setControl(driveRobot.withVelocityX(x.getAsDouble() * maxSpeed) .withVelocityY(y.getAsDouble() * maxSpeed) - .withRotationalRate(rot.getAsDouble() * maxAngularRate))); + .withRotationalRate(rot.getAsDouble() * maxAngularRate).withDriveRequestType(DriveRequestType.Velocity))); } /** @@ -234,7 +236,7 @@ public Command applyPercentRequestRobot(DoubleSupplier x, DoubleSupplier y, * @param rot the rotational velocity in rad/s */ public void setRobot(double x, double y, double rot) { - this.setControl(driveRobot.withVelocityX(x).withVelocityY(y).withRotationalRate(rot)); + this.setControl(driveRobot.withVelocityX(x).withVelocityY(y).withRotationalRate(rot).withDriveRequestType(DriveRequestType.Velocity)); } /** @@ -285,13 +287,14 @@ private void configurePathPlanner() { AutoBuilder.configureHolonomic(() -> getPose(), // Supplier of current robot pose this::seedFieldRelative, // Consumer for seeding pose against auto this::getCurrentRobotChassisSpeeds, - (speeds) -> this.setControl(autoRequest.withSpeeds(speeds)), // Consumer of - // ChassisSpeeds to - // drive the robot - new HolonomicPathFollowerConfig(AutonomousConstants.TRANSLATION_PID, + (speeds) -> this.setControl(autoRequest.withSpeeds(speeds) + .withDriveRequestType(DriveRequestType.Velocity)), // Consumer of ChassisSpeeds to drive the robot + new HolonomicPathFollowerConfig( + AutonomousConstants.TRANSLATION_PID, AutonomousConstants.ROTATION_PID, AutonomousConstants.MAX_MODULE_VELOCITY, - AutonomousConstants.DRIVE_BASE_RADIUS, new ReplanningConfig(), + AutonomousConstants.DRIVE_BASE_RADIUS, + AutonomousConstants.REPLANNING_CONFIG, AutonomousConstants.CONTROL_LOOP_PERIOD), () -> { // Boolean supplier that controls when the path will be mirrored for the red From e35706320edd00aae369e539e7c58896f75d38bf Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Wed, 3 Apr 2024 13:11:33 -0400 Subject: [PATCH 2/2] [#516] Seems stable and consistent in auto now --- .../pathplanner/autos/3-SB-[F5-F4]-EC.auto | 4 ++-- src/main/deploy/pathplanner/paths/BB-F4.path | 8 ++++---- src/main/deploy/pathplanner/paths/BF-F4.path | 8 ++++---- src/main/deploy/pathplanner/paths/BS-F4.path | 8 ++++---- src/main/deploy/pathplanner/paths/F4-BS.path | 8 ++++---- src/main/deploy/pathplanner/paths/F4-TL.path | 8 ++++---- src/main/deploy/pathplanner/paths/F5-F4.path | 8 ++++---- .../pathplanner/paths/Linked Point.path | 12 +++++------ src/main/deploy/pathplanner/paths/SB-F4.path | 10 +++++----- src/main/java/frc/robot/Constants.java | 6 +++--- src/main/java/frc/robot/RobotContainer.java | 20 +++++++++---------- .../java/frc/robot/subsystems/Indexer.java | 6 +----- 12 files changed, 51 insertions(+), 55 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/3-SB-[F5-F4]-EC.auto b/src/main/deploy/pathplanner/autos/3-SB-[F5-F4]-EC.auto index e8ed9f3e..674b644d 100644 --- a/src/main/deploy/pathplanner/autos/3-SB-[F5-F4]-EC.auto +++ b/src/main/deploy/pathplanner/autos/3-SB-[F5-F4]-EC.auto @@ -101,7 +101,7 @@ { "type": "wait", "data": { - "waitTime": 1.0 + "waitTime": 5.0 } } ] @@ -215,7 +215,7 @@ { "type": "wait", "data": { - "waitTime": 1.0 + "waitTime": 5.0 } } ] diff --git a/src/main/deploy/pathplanner/paths/BB-F4.path b/src/main/deploy/pathplanner/paths/BB-F4.path index 89bbafdc..8c5bc6f9 100644 --- a/src/main/deploy/pathplanner/paths/BB-F4.path +++ b/src/main/deploy/pathplanner/paths/BB-F4.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.145425060080891, - "y": 2.091791875891445 + "x": 6.918485375432291, + "y": 1.9437877337293141 }, "prevControl": { - "x": 4.390704122130292, - "y": 1.0570437535037265 + "x": 4.163764437481692, + "y": 0.9090396113415957 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/BF-F4.path b/src/main/deploy/pathplanner/paths/BF-F4.path index e47abf66..beea3038 100644 --- a/src/main/deploy/pathplanner/paths/BF-F4.path +++ b/src/main/deploy/pathplanner/paths/BF-F4.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.145425060080891, - "y": 2.091791875891445 + "x": 6.918485375432291, + "y": 1.9437877337293141 }, "prevControl": { - "x": 4.390704122130292, - "y": 1.0570437535037265 + "x": 4.163764437481692, + "y": 0.9090396113415957 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/BS-F4.path b/src/main/deploy/pathplanner/paths/BS-F4.path index 1127184e..8e511208 100644 --- a/src/main/deploy/pathplanner/paths/BS-F4.path +++ b/src/main/deploy/pathplanner/paths/BS-F4.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.145425060080891, - "y": 2.091791875891445 + "x": 6.918485375432291, + "y": 1.9437877337293141 }, "prevControl": { - "x": 5.795385184537515, - "y": 0.9312367137847419 + "x": 5.568445499888915, + "y": 0.7832325716226112 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/F4-BS.path b/src/main/deploy/pathplanner/paths/F4-BS.path index 9c28a07f..5e68c37c 100644 --- a/src/main/deploy/pathplanner/paths/F4-BS.path +++ b/src/main/deploy/pathplanner/paths/F4-BS.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.145425060080891, - "y": 2.091791875891445 + "x": 6.918485375432291, + "y": 1.9437877337293141 }, "prevControl": null, "nextControl": { - "x": 6.0205935796487, - "y": 1.332037279459176 + "x": 5.7936538950001, + "y": 1.1840331372970452 }, "isLocked": false, "linkedName": "F4" diff --git a/src/main/deploy/pathplanner/paths/F4-TL.path b/src/main/deploy/pathplanner/paths/F4-TL.path index f97650aa..9beef509 100644 --- a/src/main/deploy/pathplanner/paths/F4-TL.path +++ b/src/main/deploy/pathplanner/paths/F4-TL.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.145425060080891, - "y": 2.091791875891445 + "x": 6.918485375432291, + "y": 1.9437877337293141 }, "prevControl": null, "nextControl": { - "x": 5.841144715319172, - "y": 2.5173512895034134 + "x": 5.614205030670572, + "y": 2.3693471473412826 }, "isLocked": false, "linkedName": "F4" diff --git a/src/main/deploy/pathplanner/paths/F5-F4.path b/src/main/deploy/pathplanner/paths/F5-F4.path index 84c62f2b..694e755e 100644 --- a/src/main/deploy/pathplanner/paths/F5-F4.path +++ b/src/main/deploy/pathplanner/paths/F5-F4.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.145425060080891, - "y": 2.091791875891445 + "x": 6.918485375432291, + "y": 1.9437877337293141 }, "prevControl": { - "x": 7.122003386989325, - "y": 1.5530933947854944 + "x": 6.8950637023407255, + "y": 1.4050892526233636 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Linked Point.path b/src/main/deploy/pathplanner/paths/Linked Point.path index 34298795..a61a8556 100644 --- a/src/main/deploy/pathplanner/paths/Linked Point.path +++ b/src/main/deploy/pathplanner/paths/Linked Point.path @@ -176,16 +176,16 @@ }, { "anchor": { - "x": 7.145425060080891, - "y": 2.091791875891445 + "x": 6.918485375432291, + "y": 1.9437877337293141 }, "prevControl": { - "x": 7.144447301169013, - "y": 2.097718788625038 + "x": 6.917507616520413, + "y": 1.9497146464629074 }, "nextControl": { - "x": 7.146402818992769, - "y": 2.0858649631578516 + "x": 6.919463134344169, + "y": 1.9378608209957209 }, "isLocked": false, "linkedName": "F4" diff --git a/src/main/deploy/pathplanner/paths/SB-F4.path b/src/main/deploy/pathplanner/paths/SB-F4.path index 7269d998..c8a53786 100644 --- a/src/main/deploy/pathplanner/paths/SB-F4.path +++ b/src/main/deploy/pathplanner/paths/SB-F4.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.145425060080891, - "y": 2.091791875891445 + "x": 6.918485375432291, + "y": 1.9437877337293141 }, "prevControl": { - "x": 3.542147040400743, - "y": 0.3071560998822681 + "x": 4.362947187432837, + "y": 0.9570934526484439 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 28.300429472193937, + "rotation": 16.92751306414693, "rotateFast": false }, "reversed": false, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f01dac32..1f475cb7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -154,7 +154,7 @@ public static class ButtonBox { } public static class AutonomousConstants { - public static final PIDConstants TRANSLATION_PID = new PIDConstants(10, 0, 0); // TODO Tune + public static final PIDConstants TRANSLATION_PID = new PIDConstants(10, 0, 1); // TODO Tune public static final PIDConstants ROTATION_PID = new PIDConstants(4, 0, 0); // TODO: Tune public static final double MAX_MODULE_VELOCITY = Units.feetToMeters(16); // f/s to m/s @@ -226,7 +226,7 @@ public static class TunerConstants { public static final double kDriveGearRatio = 6.122448979591837; public static final double kSteerGearRatio = 21.428571428571427; - public static final double kWheelRadiusInches = 1.875; + public static final double kWheelRadiusInches = 2; private static final boolean kSteerMotorReversed = true; private static final boolean kInvertLeftSide = false; @@ -486,7 +486,7 @@ public class FlywheelConstants { public class IndexerConstants { // TODO: get real public static final boolean MOTOR_INVERT = true; - public static final int MOTOR_STATOR_CURRENT_LIMIT = 60; + public static final int MOTOR_STATOR_CURRENT_LIMIT = 100; public enum PieceState { IN_COLLECT, IN_PIVOT, IN_INDEXER, NONE diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 37be2d19..9eb86a24 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -112,16 +112,6 @@ public class RobotContainer extends LightningContainer { @Override protected void initializeSubsystems() { - boolean setPath = SignalLogger.setPath(Constants.HOOT_PATH).isOK(); - SignalLogger.enableAutoLogging(true); // TODO Return during COMPS - boolean startedLogs = SignalLogger.start().isOK(); - - if (startedLogs && setPath) { - System.out.println("STARTED HOOT LOG"); - } else { - System.out.println("FAILED TO START HOOT LOG"); - } - driver = new XboxControllerFilter(ControllerConstants.DriverControllerPort, Constants.ControllerConstants.DEADBAND, -1, 1, XboxControllerFilter.filterMode.SQUARED); // Driver controller @@ -150,6 +140,16 @@ protected void initializeSubsystems() { logger = new Telemetry(DrivetrainConstants.MaxSpeed); triggerInit = false; + + boolean setPath = SignalLogger.setPath(Constants.HOOT_PATH).isOK(); + SignalLogger.enableAutoLogging(true); // TODO Return during COMPS + boolean startedLogs = SignalLogger.start().isOK(); + + if (startedLogs && setPath) { + System.out.println("STARTED HOOT LOG"); + } else { + System.out.println("FAILED TO START HOOT LOG"); + } } @Override diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java index 2cc93988..ce9cf89f 100644 --- a/src/main/java/frc/robot/subsystems/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer.java @@ -55,11 +55,7 @@ public Indexer(Collector collector) { motor = new ThunderBird(CAN.INDEXER_MOTOR, CAN.CANBUS_FD, IndexerConstants.MOTOR_INVERT, IndexerConstants.MOTOR_STATOR_CURRENT_LIMIT, IndexerConstants.INDEXER_MOTOR_BRAKE_MODE); - motor.configSupplyLimit(0d); - motor.configStatorLimit(80d); - - motor.applyConfig(); - + initLogging(); }