Merge pull request #20 from Gongoliers/2_11_2024_drive_testing

2/11/2024 Drive testing

simgui.json

@@ -34,6 +34,9 @@
     }
   },
   "NetworkTables": {
+    "Retained Values": {
+      "open": false
+    },
     "transitory": {
       "Shuffleboard": {
         "Arm": {

src/main/deploy/pathplanner/autos/+5 spin 90 -5.auto

@@ -0,0 +1,37 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 2.0,
+      "y": 7.0
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "+5 spin 90"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 1.0
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "-5m"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/+5m -5m.auto

@@ -0,0 +1,37 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 2,
+      "y": 7.0
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "+5m"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 1.0
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "-5m"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Calibration.auto → src/main/deploy/pathplanner/autos/+5m.auto

@@ -2,7 +2,7 @@
   "version": 1.0,
   "startingPose": {
     "position": {
-      "x": 2.0,
+      "x": 2,
       "y": 7.0
     },
     "rotation": 0
@@ -14,7 +14,7 @@
         {
           "type": "path",
           "data": {
-            "pathName": "3m Backwards"
+            "pathName": "+5m"
           }
         }
       ]

src/main/deploy/pathplanner/paths/+5 spin 90.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.0,
+        "y": 7.0
+      },
+      "prevControl": {
+        "x": 6.0,
+        "y": 7.0
+      },
+      "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": -90.0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/3m Backwards.path → src/main/deploy/pathplanner/paths/+5m.path

@@ -16,11 +16,11 @@
     },
     {
       "anchor": {
-        "x": 5.0,
+        "x": 7.0,
         "y": 7.0
       },
       "prevControl": {
-        "x": 3.479309367425449,
+        "x": 6.88,
         "y": 7.0
       },
       "nextControl": null,
@@ -45,5 +45,5 @@
   "reversed": false,
   "folder": null,
   "previewStartingState": null,
-  "useDefaultConstraints": false
+  "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/-5m.path

@@ -0,0 +1,49 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.0,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 8.118033988749895,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": {
+        "x": 1.88,
+        "y": 7.0
+      },
+      "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": true
+}

src/main/java/frc/robot/arm/ArmFactory.java

@@ -11,7 +11,7 @@ public class ArmFactory {
    * @return a shoulder motor.
    */
   public static ShoulderMotorIO createShoulderMotor() {
-    if (Robot.isReal()) return new ShoulderMotorIOSparkMax();
+    // if (Robot.isReal()) return new ShoulderMotorIOSparkMax();
 
     return new ShoulderMotorIOSim();
   }

src/main/java/frc/robot/auto/Auto.java

@@ -26,20 +26,30 @@ public class Auto extends Subsystem {
   /** Instance variable for the auto subsystem singleton. */
   private static Auto instance = null;
 
+  /** Reference to the odometry subsystem. */
+  private final Odometry odometry;
+
+  /** Reference to the swerve subsystem. */
+  private final Swerve swerve;
+
+  /** Sendable chooser for the subsystem. */
   private final SendableChooser<Command> autoChooser;
 
   /** Creates a new instance of the auto subsystem. */
   private Auto() {
-    Supplier<Pose2d> robotPositionSupplier = () -> Odometry.getInstance().getPosition();
+    odometry = Odometry.getInstance();
+    swerve = Swerve.getInstance();
+
+    Supplier<Pose2d> robotPositionSupplier = () -> odometry.getPosition();
 
     Consumer<Pose2d> robotPositionConsumer =
-        position -> Odometry.getInstance().setPosition(position);
+        position -> odometry.setPosition(position);
 
     Supplier<ChassisSpeeds> swerveChassisSpeedsSupplier =
-        () -> Swerve.getInstance().getChassisSpeeds();
+        () -> swerve.getChassisSpeeds();
 
     Consumer<ChassisSpeeds> swerveChassisSpeedsConsumer =
-        chassisSpeeds -> Swerve.getInstance().setChassisSpeeds(chassisSpeeds);
+        chassisSpeeds -> swerve.setChassisSpeeds(chassisSpeeds);
 
     HolonomicPathFollowerConfig holonomicPathFollowerConfig =
         new HolonomicPathFollowerConfig(
@@ -56,7 +66,7 @@ private Auto() {
         swerveChassisSpeedsConsumer,
         holonomicPathFollowerConfig,
         this::shouldFlipPath,
-        this);
+        swerve);
 
     autoChooser = AutoBuilder.buildAutoChooser();
   }

src/main/java/frc/robot/lights/LightsFactory.java

@@ -1,5 +1,7 @@
 package frc.robot.lights;
 
+import frc.robot.Robot;
+
 /** Helper class for creating hardware for the lights subsystem. */
 public class LightsFactory {
 
@@ -9,7 +11,7 @@ public class LightsFactory {
    * @return an LED controller.
    */
   public static LEDControllerIO createLEDController() {
-    // if (Robot.isReal()) return new LEDControllerIOCANdle();
+    if (Robot.isReal()) return new LEDControllerIOCANdle();
 
     return new LEDControllerIOSim();
   }

src/main/java/frc/robot/odometry/OdometryFactory.java

@@ -2,6 +2,7 @@
 
 import com.ctre.phoenix6.configs.Pigeon2Configuration;
 import frc.lib.CAN;
+import frc.robot.Robot;
 
 /** Helper class for creating hardware for the odometry subsystem. */
 public class OdometryFactory {
@@ -12,7 +13,7 @@ public class OdometryFactory {
    * @return a gyroscope.
    */
   public static GyroscopeIO createGyroscope(CAN gyroscopeCAN, Odometry odometry) {
-    // if (Robot.isReal()) return new GyroscopeIOPigeon2(gyroscopeCAN);
+    if (Robot.isReal()) return new GyroscopeIOPigeon2(gyroscopeCAN);
 
     return new GyroscopeIOSim(odometry);
   }

src/main/java/frc/robot/swerve/DriveCommand.java

@@ -53,7 +53,13 @@ public DriveCommand(CommandXboxController driverController) {
   }
 
   @Override
-  public void initialize() {}
+  public void initialize() {
+    Rotation2d poseHeading = odometry.getPosition().getRotation();
+
+    // TODO Determine which of these, if not both, need to be set to prevent immediate spinning
+    headingGoal = wrap(new State(poseHeading.getRotations(), 0.0), poseHeading.getRotations());
+    headingSetpoint = wrap(new State(poseHeading.getRotations(), 0.0), poseHeading.getRotations());
+  }
 
   @Override
   public void execute() {

src/main/java/frc/robot/swerve/SwerveConstants.java

@@ -25,14 +25,14 @@ public static class MK4iConstants {
     public static final double WHEEL_DIAMETER = Units.inchesToMeters(4.0);
 
     /** Actual distance driven during the odometry test in meters. */
-    public static final double ODOMETRY_TEST_ACTUAL_DISTANCE = 6.985;
+    public static final double ODOMETRY_TEST_ACTUAL_DISTANCE = 5.49;
 
     /** Reported distance driven during the odometry test in meters. */
-    public static final double ODOMETRY_TEST_REPORTED_DISTANCE = 8.219;
+    public static final double ODOMETRY_TEST_REPORTED_DISTANCE = 5.0;
 
     /** Conversion between odometry distance and actual distance travelled. */
     public static final double ODOMETRY_ERROR =
-        ODOMETRY_TEST_ACTUAL_DISTANCE / ODOMETRY_TEST_REPORTED_DISTANCE;
+        ODOMETRY_TEST_REPORTED_DISTANCE / ODOMETRY_TEST_ACTUAL_DISTANCE;
 
     /** Conversion between wheel rotations and distances in meters. */
     public static final double WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER * Math.PI * ODOMETRY_ERROR;

src/main/java/frc/robot/swerve/SwerveFactory.java

@@ -23,8 +23,8 @@ public static SwerveModuleIO createModule(SwerveModuleConfig config) {
    * @return an azimuth encoder.
    */
   public static AzimuthEncoderIO createAzimuthEncoder(SwerveModuleConfig config) {
-    // if (Robot.isReal())
-    //   return new AzimuthEncoderIOCANcoder(config.moduleCAN().azimuth(), config.offset());
+    if (Robot.isReal())
+      return new AzimuthEncoderIOCANcoder(config.moduleCAN().azimuth(), config.offset());
 
     return new AzimuthEncoderIOSim();
   }
@@ -46,9 +46,9 @@ public static CANcoderConfiguration createAzimuthEncoderConfig() {
    * @return a steer motor.
    */
   public static SteerMotorIO createSteerMotor(SwerveModuleConfig config) {
-    // if (Robot.isReal())
-    //   return new SteerMotorIOTalonFXPIDF(config.moduleCAN().steer(),
-    // config.moduleCAN().azimuth());
+    if (Robot.isReal())
+      return new SteerMotorIOTalonFXPIDF(config.moduleCAN().steer(),
+    config.moduleCAN().azimuth());
 
     return new SteerMotorIOSim();
   }
@@ -59,7 +59,7 @@ public static SteerMotorIO createSteerMotor(SwerveModuleConfig config) {
    * @return a drive motor.
    */
   public static DriveMotorIO createDriveMotor(SwerveModuleConfig config) {
-    // if (Robot.isReal()) return new DriveMotorIOTalonFXPID(config.moduleCAN().drive());
+    if (Robot.isReal()) return new DriveMotorIOTalonFXPID(config.moduleCAN().drive());
 
     return new DriveMotorIOSim();
   }