changing a bunch of constants and fixing stuff

src/main/deploy/pathplanner/paths/300 Right to HP.path

@@ -0,0 +1,72 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6061643835666786,
+        "y": 5.031720890416444
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.065239726031196,
+        "y": 5.647773972598556
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.2510679859194649,
+        "y": 7.0812357195448
+      },
+      "prevControl": {
+        "x": 1.6678510274010594,
+        "y": 6.6394691780780075
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.498997995991984,
+      "rotationDegrees": -52.46519085612145
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Score L4",
+      "waypointRelativePos": 0,
+      "endWaypointRelativePos": null,
+      "command": null
+    },
+    {
+      "name": "HP Pickup",
+      "waypointRelativePos": 0.16666666666666663,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -53.98486432191523
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -59.99999999999999
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/HP to 300 Left.path

@@ -0,0 +1,61 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.3072345890410957,
+        "y": 7.135316780821918
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.5994434931506847,
+        "y": 6.909931506849315
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.026883561643835,
+        "y": 5.257106164383561
+      },
+      "prevControl": {
+        "x": 3.7113441780821916,
+        "y": 5.783005136986301
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Score L4",
+      "waypointRelativePos": 0,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -59.69923999693802
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -53.97262661489646
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/settings.json

@@ -0,0 +1,32 @@
+{
+  "robotWidth": 0.8763,
+  "robotLength": 0.8763,
+  "holonomicMode": true,
+  "pathFolders": [],
+  "autoFolders": [],
+  "defaultMaxVel": 4.0,
+  "defaultMaxAccel": 4.0,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "defaultNominalVoltage": 12.0,
+  "robotMass": 48.35,
+  "robotMOI": 6.883,
+  "robotTrackwidth": 0.546,
+  "driveWheelRadius": 0.038,
+  "driveGearing": 4.29,
+  "maxDriveSpeed": 5.45,
+  "driveMotorType": "krakenX60",
+  "driveCurrentLimit": 65.0,
+  "wheelCOF": 1.1,
+  "flModuleX": 0.31115,
+  "flModuleY": 0.31115,
+  "frModuleX": 0.31115,
+  "frModuleY": -0.31115,
+  "blModuleX": -0.31115,
+  "blModuleY": 0.31115,
+  "brModuleX": -0.31115,
+  "brModuleY": -0.31115,
+  "bumperOffsetX": 0.0,
+  "bumperOffsetY": 0.0,
+  "robotFeatures": []
+}

src/main/java/frc/robot/constants/AutoConstants.java

@@ -11,8 +11,8 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 
 public class AutoConstants {
-    public static final double kMaxSpeedMetersPerSecond = 3;
+    public static final double kMaxSpeedMetersPerSecond = 4;
-    public static final double kMaxAccelerationMetersPerSecondSquared = 3;
+    public static final double kMaxAccelerationMetersPerSecondSquared = 4;
     public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
     public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
 

src/main/java/frc/robot/constants/DrivetrainConstants.java

@@ -17,9 +17,9 @@ public class DrivetrainConstants {
     public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second
 
     // Chassis configuration
-    public static final double kTrackWidth = Units.inchesToMeters(26.5);
+    public static final double kTrackWidth = Units.inchesToMeters(24.5);
     // Distance between centers of right and left wheels on robot
-    public static final double kWheelBase = Units.inchesToMeters(26.5);
+    public static final double kWheelBase = Units.inchesToMeters(24.5);
 
     // Angular offsets of the modules relative to the chassis in radians
     public static final double kFrontLeftChassisAngularOffset = -Math.PI / 2;
@@ -48,7 +48,7 @@ public class DrivetrainConstants {
     public static final int kFrontRightTurningCanId = 14;
     public static final int kRearRightTurningCanId = 16;
 
-    public static final boolean kGyroReversed = false;
+    public static final boolean kGyroReversed = true;
 
     // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM
 

src/main/java/frc/robot/constants/ElevatorConstants.java

@@ -19,7 +19,7 @@ public class ElevatorConstants {
 
     public static final double kEncoderConversionFactor = 0;
 
-    public static final int kMotorAmpsMax = 0;
+    public static final int kMotorAmpsMax = 40;
 
     public static final double kPositionControllerP = 0;
     public static final double kPositionControllerI = 0;
@@ -30,8 +30,8 @@ public class ElevatorConstants {
     public static final double kVelocityControllerD = 0;
 
     public static final double kFeedForwardS = 0;
-    public static final double kFeedForwardG = 0;
+    public static final double kFeedForwardG = 0.41; // calculated value
-    public static final double kFeedForwardV = 0;
+    public static final double kFeedForwardV = 1.75; // calculated value
 
     public static final double kMaxVelocity = 0;
 

src/main/java/frc/robot/constants/ManipulatorPivotConstants.java

@@ -17,7 +17,7 @@ public class ManipulatorPivotConstants {
     public static final int kArmMotorID = 0;
     public static final int kCANcoderID = 0;
 
-    public static final int kMotorAmpsMax = 0;
+    public static final int kMotorAmpsMax = 40;
 
     public static final double kPivotMaxVelocity = 0;
 
@@ -29,6 +29,11 @@ public class ManipulatorPivotConstants {
     public static final double kVelocityP = 0;
     public static final double kVelocityI = 0;
     public static final double kVelocityD = 0;
+
+    public static final double kFeedForwardS = 0;
+    public static final double kFeedForwardG = 0.41; // calculated value
+    public static final double kFeedForwardV = 0.68; //calculated value
+
     // TODO Is this reasonable?
     public static final double kVelocityTolerance = Units.degreesToRadians(3) / 60;
 

src/main/java/frc/robot/constants/ModuleConstants.java

@@ -41,7 +41,8 @@ public class ModuleConstants {
     public static final double kTurnI = 0;
     public static final double kTurnD = 0;
 
-    public static final int kDriveMotorStatorCurrentLimit = 120;
+    public static final int kDriveMotorStatorCurrentLimit = 100;
+    public static final int kDriveMotorSupplyCurrentLimit = 65;
     public static final int kTurnMotorCurrentLimit = 20;
 
     public static final IdleMode kTurnIdleMode = IdleMode.kBrake;
@@ -62,7 +63,9 @@ public class ModuleConstants {
         kDriveFeedConfig.SensorToMechanismRatio = kDrivingMotorReduction;
 
         kDriveCurrentLimitConfig.StatorCurrentLimitEnable = true;
+        kDriveCurrentLimitConfig.SupplyCurrentLimitEnable = true;
         kDriveCurrentLimitConfig.StatorCurrentLimit = kDriveMotorStatorCurrentLimit;
+        kDriveCurrentLimitConfig.SupplyCurrentLimit = kDriveMotorSupplyCurrentLimit;
 
         kDriveMotorConfig.Inverted = kDriveInversionState;
         kDriveMotorConfig.NeutralMode = kDriveIdleMode;

src/main/java/frc/robot/subsystems/ManipulatorPivot.java

@@ -58,6 +58,12 @@ public class ManipulatorPivot extends SubsystemBase {
 
         canCoder = new CANcoder(ManipulatorPivotConstants.kCANcoderID);
         canCoder.getConfigurator().apply(ManipulatorPivotConstants.canCoderConfig);
+
+        feedForward = new ArmFeedforward(
+            ManipulatorPivotConstants.kFeedForwardS,
+            ManipulatorPivotConstants.kFeedForwardG, 
+            ManipulatorPivotConstants.kFeedForwardV
+            );
     }
 
     /**

src/main/java/frc/robot/subsystems/TylerVision.java

@@ -0,0 +1,46 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.networktables.BooleanSubscriber;
+import edu.wpi.first.networktables.DoubleSubscriber;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+public class TylerVision{
+
+    private DoubleSubscriber cam1GlobalPose;
+    private DoubleSubscriber cam1ClosestTag;
+    private BooleanSubscriber cam1TagDetected;
+
+    private DoubleSubscriber framerate;
+
+    public TylerVision(){
+        NetworkTableInstance inst = NetworkTableInstance.getDefault();
+
+        NetworkTable visionTable = inst.getTable("Fiducial");
+
+        cam1GlobalPose = visionTable.getDoubleTopic("cam1GlobalPose").subscribe(0.0);
+        cam1ClosestTag = visionTable.getDoubleTopic("cam1ClosestTag").subscribe(0.0);
+
+        cam1TagDetected = visionTable.getBooleanTopic("cam1_visible").subscribe(false);
+      //  cam2TagDetected = visionTable.getBooleanTopic("cam2_visible").subscribe(false);
+
+        framerate = visionTable.getDoubleTopic("fps").subscribe(0.0);
+    }
+
+    public double getCam1GlobalPose(){
+        return cam1GlobalPose.get();
+    }
+
+    public double getCam1ClosestTag(){
+        return cam1ClosestTag.get();
+    }
+
+    public boolean getCam1TagDetected(){
+        return cam1TagDetected.get();
+    }
+
+    public double getFPS(){
+        return framerate.get();
+    }
+    
+}