Switch to Spark Max + Encoders + Built in PID for auto

.DataLogTool/datalogtool.json

@@ -0,0 +1,6 @@
+{
+  "download": {
+    "localDir": "C:\\Users\\BeachBots\\Downloads",
+    "serverTeam": "10.76.52.2"
+  }
+}

.SysId/sysid.json

@@ -0,0 +1 @@
+{}

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2024.1.1"
+    id "edu.wpi.first.GradleRIO" version "2024.2.1"
     id 'com.diffplug.spotless' version '6.20.0'
     id "com.peterabeles.gversion" version "1.10"
 }

src/main/deploy/pathplanner/autos/DriveForward.auto

@@ -0,0 +1,31 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 2,
+      "y": 7.0
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "DriveForward"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BrakeCommand"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/DriveForward.path

@@ -0,0 +1,49 @@
+{
+  "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": 3.25,
+        "y": 7.0
+      },
+      "prevControl": {
+        "x": 2.25,
+        "y": 7.0
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.0,
+    "maxAcceleration": 0.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": null,
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/Example Path.path

@@ -54,7 +54,7 @@
     "maxAngularAcceleration": 720.0
   },
   "goalEndState": {
-    "velocity": 0,
+    "velocity": 0.0,
     "rotation": 0,
     "rotateFast": false
   },

src/main/java/frc/robot/Constants.java

@@ -15,22 +15,27 @@ public final class Constants {
   public static final class CANConstants {
     // CAN Bus Devices
     /// Drive Train Motors
-    public static final int MOTORFRONTRIGHTID = 11;
-    public static final int MOTORBACKRIGHTID = 12;
-    public static final int MOTORFRONTLEFTID = 13;
-    public static final int MOTORBACKLEFTID = 14;
+    public static final int MOTORFRONTRIGHTID = 14;
+    public static final int MOTORBACKRIGHTID = 13;
+    public static final int MOTORFRONTLEFTID = 12;
+    public static final int MOTORBACKLEFTID = 11;
     /// Arm Motors
     public static final int MOTORARMMAINID = 21;
-    public static final int MOTORARMSECONDARYID = 22;
     /// Shooter Motors
     public static final int MOTORSHOOTERID = 31;
+    /// Lifter Motors
+    public static final int MOTORLIFTERLEFTID = 41;
+    public static final int MOTORLIFTERRIGHTID = 42;
   }
 
   public static final double MAX_SPEED = 0.8;
+  public static final double LIFTERSPEED = 0.5;
 
   // USB Devices
   public static final int CONTROLLERUSBINDEX = 0;
   public static final int FLIGHTSTICKUSBINDEX = 1;
+  // On-Controller joystick deadzone
+  public static final double CONTROLLERDEADZONE = 0.1;
 
   // Game Controller Buttons
   // Now In RobotContainer as Native Triggers.
@@ -44,9 +49,4 @@ public static final class CANConstants {
   public static final int ULTRASONICSHOOTERPORT = 0;
 
   // Digital Ports
-  /// Encoder Ports
-  public static final int DRIVEENCODERLEFTA = 0;
-  public static final int DRIVEENCODERLEFTB = 1;
-  public static final int DRIVEENCODERRIGHTA = 2;
-  public static final int DRIVEENCODERRIGHTB = 3;
 }

src/main/java/frc/robot/DriveConstants.java

@@ -1,7 +1,6 @@
 package frc.robot;
 
 import com.pathplanner.lib.util.ReplanningConfig;
-import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
 import edu.wpi.first.math.util.Units;
 
@@ -13,29 +12,49 @@
  */
 public final class DriveConstants {
   // general drive constants
+  // https://www.chiefdelphi.com/t/encoders-velocity-to-m-s/390332/2
+  // https://sciencing.com/convert-rpm-linear-speed-8232280.html
   public static final double WHEEL_DIAMETER = Units.inchesToMeters(6); // meters
-  public static final double PULSES_PER_REV = 2048; // resolution of encoder
-  public static final double GEAR_RATIO = 1;
-  public static final double DISTANCE_PER_PULSE =
+  public static final double kTrackwidthMeters = 0.60048;
+  // this is not used and is handled by the rev encoder.
+  public static final double PULSES_PER_REV = 1;
+  public static final double GEAR_RATIO = 8.46; // 8.46:1
+  // basically converted from rotations to to radians to then meters using the wheel diameter.
+  // the diameter is already *2 so we don't need to multiply by 2 again.
+  public static final double POSITION_CONVERSION_RATIO =
       (Math.PI * WHEEL_DIAMETER) / PULSES_PER_REV / GEAR_RATIO;
+  public static final double VELOCITY_CONVERSION_RATIO = POSITION_CONVERSION_RATIO / 60;
   // Kinematic constants
 
   // These characterization values MUST be determined either experimentally or theoretically
   // for *your* robot's drive.
   // The Robot Characterization Toolsuite provides a convenient tool for obtaining these
   // values for your robot.
   // Feed Forward Constants
-  public static final double Ks = 0.76856; // volts
-  public static final double Kv = 2.4467; // VoltSecondsPerMeter
-  public static final double Ka = 0.58646; // VoltSecondsSquaredPerMeter
-  public static final SimpleMotorFeedforward FeedForward = new SimpleMotorFeedforward(Ks, Kv, Ka);
+  public static final double ksDriveVolts = 0.16213;
+  public static final double kvDriveVoltSecondsPerMeter = 2.2194;
+  public static final double kaDriveVoltSecondsSquaredPerMeter = 0.33599;
+  // Max speed Constants
+  public static final double kMaxOutputDrive = 1.0;
+  public static final double kMinOutputDrive = -1.0;
   // Feed Back / PID Constants
-  public static final double kPDriveVel = 3.6293;
+  public static final double kPDriveVel = 0.00034388;
+  public static final double kIDriveVel = 0.0;
+  public static final double kDDriveVel = 0.0;
+  public static final double kIzDriveVel = 0.0; // error before integral takes effect
+
+  public static final double kPDrivePos = 5.7745;
+  public static final double kIDrivePos = 0.0;
+  public static final double kDDrivePos = 0.55289;
+  public static final double kIzDrivePos = 0.0; // error before integral takes effect
   // Helper class that converts a chassis velocity (dx and dtheta components) to left and right
   // wheel velocities for a differential drive.
-  public static final double kTrackwidthMeters = 0.60048;
   public static final DifferentialDriveKinematics kDriveKinematics =
       new DifferentialDriveKinematics(kTrackwidthMeters);
   // Default path replanning config. See the API for the options
   public static final ReplanningConfig autoReplanningConfig = new ReplanningConfig();
+
+  // Motor Controller PID Slots
+  public static final int kDrivetrainVelocityPIDSlot = 0;
+  public static final int kDrivetrainPositionPIDSlot = 1;
 }

src/main/java/frc/robot/Robot.java

@@ -4,15 +4,12 @@
 
 package frc.robot;
 
-import edu.wpi.first.cameraserver.CameraServer;
-import edu.wpi.first.cscore.UsbCamera;
-import edu.wpi.first.cscore.VideoSink;
+import edu.wpi.first.wpilibj.DataLogManager;
 // import edu.wpi.first.cscore.VideoSource.ConnectionStrategy;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
+import org.littletonrobotics.urcl.URCL;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -25,11 +22,6 @@ public class Robot extends TimedRobot {
 
   private RobotContainer m_robotContainer;
 
-  private UsbCamera camera1;
-  private UsbCamera camera2;
-  private VideoSink mainCameraServer;
-  private int cameraCounter = 2;
-
   /**
    * This function is run when the robot is first started up and should be used for any
    * initialization code.
@@ -40,28 +32,14 @@ public void robotInit() {
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
 
-    camera1 = CameraServer.startAutomaticCapture(0);
-    camera2 = CameraServer.startAutomaticCapture(1);
-    mainCameraServer = CameraServer.getServer();
     // Tell both cameras to always stream.
     // camera1.setConnectionStrategy(ConnectionStrategy.kKeepOpen);
     // camera2.setConnectionStrategy(ConnectionStrategy.kKeepOpen);
 
-    // this is to put git info in the dashboard & Logs, uses new 2024 BuildConstants.java
-    String branchName = BuildConstants.GIT_BRANCH;
-    String commitHash = BuildConstants.GIT_SHA;
-    String commitTime = BuildConstants.GIT_DATE;
-    String buildTime = BuildConstants.BUILD_DATE;
-
-    System.out.println("Branch: " + branchName);
-    System.out.println("Commit: " + commitHash);
-    System.out.println("Commit Time: " + commitTime);
-    System.out.println("Build Time: " + buildTime);
-
-    SmartDashboard.putString("Branch", branchName);
-    SmartDashboard.putString("Commit", commitHash);
-    SmartDashboard.putString("CommitTime", commitTime);
-    SmartDashboard.putString("BuildTime", buildTime);
+    if (m_robotContainer.enableAutoProfiling) {
+      DataLogManager.start();
+      URCL.start();
+    }
   }
 
   /**
@@ -78,6 +56,9 @@ public void robotPeriodic() {
     // and running subsystem periodic() methods.  This must be called from the robot's periodic
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
+    if (m_robotContainer.enableAutoProfiling) {
+      System.out.println("WARNING, AUTO PROFILE IS ENABLED!");
+    }
   }
 
   /** This function is called once each time the robot enters Disabled mode. */
@@ -111,20 +92,6 @@ public void teleopInit() {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
-    m_robotContainer
-        .getCameraButton()
-        .onTrue(
-            new InstantCommand(
-                () -> {
-                  cameraCounter++;
-                  if (cameraCounter % 2 == 0) {
-                    System.out.println("Setting Camera 2");
-                    mainCameraServer.setSource(camera2);
-                  } else {
-                    System.out.println("Setting Camera 1");
-                    mainCameraServer.setSource(camera1);
-                  }
-                }));
   }
 
   /** This function is called periodically during operator control. */