[#516] Merge pull request #518 from frc-862/516-use-closed-loop-driving

[#516] use closed loop driving
frc-862 · Apr 3, 2024 · 8d9b8aa · 8d9b8aa
2 parents 878ad9f + e357063
commit 8d9b8aa
Show file tree

Hide file tree

Showing 13 changed files with 68 additions and 68 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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;
@@ -153,17 +154,17 @@ 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
         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.
@@ -225,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;
@@ -485,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
@@ -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
@@ -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();
     }
 

diff --git 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