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,13 +15,12 @@ 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;
   }
@@ -44,9 +43,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/HelperFunctions.java

@@ -0,0 +1,8 @@
+package frc.robot;
+
+public final class HelperFunctions {
+
+  public static boolean inDeadzone(double value, double deadzone) {
+    return Math.abs(value) < deadzone;
+  }
+}

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

@@ -7,12 +7,14 @@
 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
@@ -62,6 +64,11 @@ public void robotInit() {
     SmartDashboard.putString("Commit", commitHash);
     SmartDashboard.putString("CommitTime", commitTime);
     SmartDashboard.putString("BuildTime", buildTime);
+
+    if (m_robotContainer.enableAutoProfiling) {
+      DataLogManager.start();
+      URCL.start();
+    }
   }
 
   /**
@@ -78,6 +85,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. */

src/main/java/frc/robot/RobotContainer.java

@@ -10,6 +10,7 @@
 import com.pathplanner.lib.path.PathConstraints;
 import com.pathplanner.lib.path.PathPlannerPath;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
@@ -20,6 +21,7 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.commands.AimCommand;
 import frc.robot.commands.ArmCommand;
 import frc.robot.commands.BalanceCommand;
@@ -86,6 +88,7 @@ public class RobotContainer {
   // Init For Autonomous
   // private RamseteAutoBuilder autoBuilder;
   private SendableChooser<String> autoDashboardChooser = new SendableChooser<String>();
+  public final boolean enableAutoProfiling = false;
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
@@ -94,7 +97,15 @@ public RobotContainer() {
     // Setup On the Fly Path Planning
     configureTeleopPaths();
     // Configure the button bindings
-    configureButtonBindings();
+    setupTriggers();
+    // Bind the commands to the triggers
+    if (enableAutoProfiling) {
+      bindDriveSysIDCommands();
+      // bindArmSysIDCommands();
+      // bindShooterSysIDCommands();
+    } else {
+      bindCommands();
+    }
 
     // set default drive command
     m_driveSubsystem.setDefaultCommand(m_defaultDrive);
@@ -110,7 +121,7 @@ public RobotContainer() {
    * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
    * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
-  private void configureButtonBindings() {
+  private void setupTriggers() {
     // Controller buttons
     m_switchCameraButton = m_controller1.x();
     m_brakeButton = m_controller1.a();
@@ -121,6 +132,9 @@ private void configureButtonBindings() {
     // Joystick buttons
     m_aimButton = new JoystickButton(m_flightStick, Constants.AIMBUTTON);
     m_fireButton = new JoystickButton(m_flightStick, Constants.FIREBUTTON);
+  }
+
+  private void bindCommands() {
     // commands
     m_balanceButton.whileTrue(m_balanceCommand);
     m_straightButton.whileTrue(m_straightCommand);
@@ -132,9 +146,38 @@ private void configureButtonBindings() {
     m_coastButton.whileTrue(new InstantCommand(() -> m_driveSubsystem.SetCoastmode()));
   }
 
+  private void bindDriveSysIDCommands() {
+    m_controller1.a().whileTrue(m_driveSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+    m_controller1.b().whileTrue(m_driveSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+    m_controller1.x().whileTrue(m_driveSubsystem.sysIdDynamic(SysIdRoutine.Direction.kForward));
+    m_controller1.y().whileTrue(m_driveSubsystem.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+    m_controller1.leftTrigger().whileTrue(new InstantCommand(() -> DataLogManager.stop()));
+  }
+
+  private void bindArmSysIDCommands() {
+    m_controller1.a().whileTrue(m_armSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+    m_controller1.b().whileTrue(m_armSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+    m_controller1.x().whileTrue(m_armSubsystem.sysIdDynamic(SysIdRoutine.Direction.kForward));
+    m_controller1.y().whileTrue(m_armSubsystem.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+    m_controller1.leftTrigger().whileTrue(new InstantCommand(() -> DataLogManager.stop()));
+  }
+
+  private void bindShooterSysIDCommands() {
+    m_controller1
+        .a()
+        .whileTrue(m_shooterSubsytem.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+    m_controller1
+        .b()
+        .whileTrue(m_shooterSubsytem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+    m_controller1.x().whileTrue(m_shooterSubsytem.sysIdDynamic(SysIdRoutine.Direction.kForward));
+    m_controller1.y().whileTrue(m_shooterSubsytem.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+    m_controller1.leftTrigger().whileTrue(new InstantCommand(() -> DataLogManager.stop()));
+  }
+
   private void initializeAutonomous() {
     // Network Table Routine Options
     autoDashboardChooser.setDefaultOption("Auto With Balancing", "FullAuto");
+    autoDashboardChooser.setDefaultOption("DriveForward", "DriveForward");
     autoDashboardChooser.addOption("End at cones", "EndAtCones");
     autoDashboardChooser.addOption("Do Nothing", "DoNothing");
     SmartDashboard.putData(autoDashboardChooser);
@@ -144,6 +187,8 @@ private void initializeAutonomous() {
     // NamedCommands.registerCommand("A", new PathFollowingCommand(m_driveSubsystem,
     // pathGroup.get(0)));
     NamedCommands.registerCommand("BalanceRobot", m_balanceCommand);
+    NamedCommands.registerCommand(
+        "BrakeCommand", new InstantCommand(() -> m_driveSubsystem.SetBrakemode()));
 
     // autoBuilder =
     //     new RamseteAutoBuilder(

src/main/java/frc/robot/commands/AimCommand.java

@@ -53,10 +53,9 @@ public void execute() {
                   Units.degreesToRadians(CamResult.getBestTarget().getPitch()))
               - m_cameraSubsystem.frontCameraGoalRangeMeters;
       // turn and move towards target.
-      m_driveSubsystem.driveAndTurn(m_driveSubsystem.getYaw(), angleGoal, distanceFromTarget);
+      // m_driveSubsystem.driveAndTurn(m_driveSubsystem.getYaw(), angleGoal, distanceFromTarget);
       // we reset the angle everytime as the target could change / move.
       m_driveSubsystem.turnSetGoal(angleGoal);
-      m_driveSubsystem.distanceSetGoal(distanceFromTarget);
     } else {
       SmartDashboard.putBoolean("CameraTargetDetected", false);
       SmartDashboard.putNumber("CameraTargetPitch", 0.0);
@@ -68,7 +67,7 @@ public void execute() {
   @Override
   public void end(boolean interrupted) {
     m_driveSubsystem.turnResetPID(); // we clear the PID turn controller.
-    m_driveSubsystem.distanceResetPID(); // we clear the distance PID contoller too.
+    m_driveSubsystem.stop(); // end execution of on board PID.
   }
 
   // Returns true when the command should end.