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/MAXSwerveModule.java

@@ -22,120 +22,120 @@ import com.revrobotics.AbsoluteEncoder;
 import frc.robot.constants.ModuleConstants;
 
 public class MAXSwerveModule {
-  private final TalonFX m_drive;
+    private final TalonFX m_drive;
-  private final SparkMax m_turningSpark;
+    private final SparkMax m_turningSpark;
 
-  private final AbsoluteEncoder m_turningEncoder;
+    private final AbsoluteEncoder m_turningEncoder;
 
-  private final SparkClosedLoopController m_turningClosedLoopController;
+    private final SparkClosedLoopController m_turningClosedLoopController;
 
-  private final VelocityVoltage driveVelocityRequest;
+    private final VelocityVoltage driveVelocityRequest;
 
-  private double m_chassisAngularOffset = 0;
+    private double m_chassisAngularOffset = 0;
-  private SwerveModuleState m_desiredState = new SwerveModuleState(0.0, new Rotation2d());
+    private SwerveModuleState m_desiredState = new SwerveModuleState(0.0, new Rotation2d());
 
-  /**
+    /**
-   * Constructs a MAXSwerveModule and configures the driving and turning motor,
+     * Constructs a MAXSwerveModule and configures the driving and turning motor,
-   * encoder, and PID controller. This configuration is specific to the REV
+     * encoder, and PID controller. This configuration is specific to the REV
-   * MAXSwerve Module built with NEOs, SPARKS MAX, and a Through Bore
+     * MAXSwerve Module built with NEOs, SPARKS MAX, and a Through Bore
-   * Encoder.
+     * Encoder.
-   */
+     */
-  public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) {
+    public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) {
-    m_drive = new TalonFX(drivingCANId);
+        m_drive = new TalonFX(drivingCANId);
-    m_turningSpark = new SparkMax(turningCANId, MotorType.kBrushless);
+        m_turningSpark = new SparkMax(turningCANId, MotorType.kBrushless);
 
-    m_turningEncoder = m_turningSpark.getAbsoluteEncoder();
+        m_turningEncoder = m_turningSpark.getAbsoluteEncoder();
 
-    m_turningClosedLoopController = m_turningSpark.getClosedLoopController();
+        m_turningClosedLoopController = m_turningSpark.getClosedLoopController();
 
-    driveVelocityRequest = new VelocityVoltage(0).withSlot(0);
+        driveVelocityRequest = new VelocityVoltage(0).withSlot(0);
 
-    // Apply the respective configurations to the SPARKS. Reset parameters before
+        // Apply the respective configurations to the SPARKS. Reset parameters before
-    // applying the configuration to bring the SPARK to a known good state. Persist
+        // applying the configuration to bring the SPARK to a known good state. Persist
-    // the settings to the SPARK to avoid losing them on a power cycle.
+        // the settings to the SPARK to avoid losing them on a power cycle.
-    m_drive.getConfigurator().apply(ModuleConstants.kDriveCurrentLimitConfig);
+        m_drive.getConfigurator().apply(ModuleConstants.kDriveCurrentLimitConfig);
-    m_drive.getConfigurator().apply(ModuleConstants.kDriveFeedConfig);
+        m_drive.getConfigurator().apply(ModuleConstants.kDriveFeedConfig);
-    m_drive.getConfigurator().apply(ModuleConstants.kDriveMotorConfig);
+        m_drive.getConfigurator().apply(ModuleConstants.kDriveMotorConfig);
-    m_drive.getConfigurator().apply(ModuleConstants.kDriveSlot0Config);
+        m_drive.getConfigurator().apply(ModuleConstants.kDriveSlot0Config);
 
-    m_turningSpark.configure(ModuleConstants.turningConfig, ResetMode.kResetSafeParameters,
+        m_turningSpark.configure(ModuleConstants.turningConfig, ResetMode.kResetSafeParameters,
-        PersistMode.kPersistParameters);
+            PersistMode.kPersistParameters);
 
-    m_chassisAngularOffset = chassisAngularOffset;
+        m_chassisAngularOffset = chassisAngularOffset;
-    m_desiredState.angle = new Rotation2d(m_turningEncoder.getPosition());
+        m_desiredState.angle = new Rotation2d(m_turningEncoder.getPosition());
-    m_drive.setPosition(0);
+        m_drive.setPosition(0);
-  }
+    }
 
-  /**
+    /**
-   * Returns the current state of the module.
+     * Returns the current state of the module.
-   *
+     *
-   * @return The current state of the module.
+     * @return The current state of the module.
-   */
+     */
-  public SwerveModuleState getState() {
+    public SwerveModuleState getState() {
-    // Apply chassis angular offset to the encoder position to get the position
+        // Apply chassis angular offset to the encoder position to get the position
-    // relative to the chassis.
+        // relative to the chassis.
-    return new SwerveModuleState(m_drive.getVelocity().getValueAsDouble(),
+        return new SwerveModuleState(m_drive.getVelocity().getValueAsDouble(),
-        new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
+            new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
-  }
+    }
 
-  /**
+    /**
-   * Returns the current position of the module.
+     * Returns the current position of the module.
-   *
+     *
-   * @return The current position of the module.
+     * @return The current position of the module.
-   */
+     */
-  public SwerveModulePosition getPosition() {
+    public SwerveModulePosition getPosition() {
-    // Apply chassis angular offset to the encoder position to get the position
+        // Apply chassis angular offset to the encoder position to get the position
-    // relative to the chassis.
+        // relative to the chassis.
-    return new SwerveModulePosition(m_drive.getPosition().getValueAsDouble(),
+        return new SwerveModulePosition(m_drive.getPosition().getValueAsDouble(),
-        new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
+            new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
-  }
+    }
 
-  /**
+    /**
-   * Sets the desired state for the module.
+     * Sets the desired state for the module.
-   *
+     *
-   * @param desiredState Desired state with speed and angle.
+     * @param desiredState Desired state with speed and angle.
-   */
+     */
-  public void setDesiredState(SwerveModuleState desiredState) {
+    public void setDesiredState(SwerveModuleState desiredState) {
-    // Apply chassis angular offset to the desired state.
+        // Apply chassis angular offset to the desired state.
-    SwerveModuleState correctedDesiredState = new SwerveModuleState();
+        SwerveModuleState correctedDesiredState = new SwerveModuleState();
-    correctedDesiredState.speedMetersPerSecond = desiredState.speedMetersPerSecond;
+        correctedDesiredState.speedMetersPerSecond = desiredState.speedMetersPerSecond;
-    correctedDesiredState.angle = desiredState.angle.plus(Rotation2d.fromRadians(m_chassisAngularOffset));
+        correctedDesiredState.angle = desiredState.angle.plus(Rotation2d.fromRadians(m_chassisAngularOffset));
 
-    // Optimize the reference state to avoid spinning further than 90 degrees.
+        // Optimize the reference state to avoid spinning further than 90 degrees.
-    correctedDesiredState.optimize(new Rotation2d(m_turningEncoder.getPosition()));
+        correctedDesiredState.optimize(new Rotation2d(m_turningEncoder.getPosition()));
 
-    // Command driving and turning SPARKS towards their respective setpoints.
+        // Command driving and turning SPARKS towards their respective setpoints.
-    m_drive.setControl(
+        m_drive.setControl(
-      driveVelocityRequest.withVelocity(
+        driveVelocityRequest.withVelocity(
-        correctedDesiredState.speedMetersPerSecond
+            correctedDesiredState.speedMetersPerSecond
-      ).withFeedForward(
+        ).withFeedForward(
-        correctedDesiredState.speedMetersPerSecond
+            correctedDesiredState.speedMetersPerSecond
-      )
+        )
-    );
+        );
 
-    m_turningClosedLoopController.setReference(correctedDesiredState.angle.getRadians(), ControlType.kPosition);
+        m_turningClosedLoopController.setReference(correctedDesiredState.angle.getRadians(), ControlType.kPosition);
 
-    m_desiredState = desiredState;
+        m_desiredState = desiredState;
-  }
+    }
 
-  public void setVoltageDrive(double voltage){
+    public void setVoltageDrive(double voltage){
-    m_drive.setVoltage(voltage);
+        m_drive.setVoltage(voltage);
-  }
+    }
 
-  public void setVoltageTurn(double voltage) {
+    public void setVoltageTurn(double voltage) {
-    m_turningSpark.setVoltage(voltage);
+        m_turningSpark.setVoltage(voltage);
-  }
+    }
 
-  public double getVoltageDrive() {
+    public double getVoltageDrive() {
-    return m_drive.get() * RobotController.getBatteryVoltage();
+        return m_drive.get() * RobotController.getBatteryVoltage();
-  }
+    }
 
-  public double getVoltageTurn() {
+    public double getVoltageTurn() {
-    return m_turningSpark.get() * RobotController.getBatteryVoltage();
+        return m_turningSpark.get() * RobotController.getBatteryVoltage();
-  }
+    }
 
-  /** Zeroes all the SwerveModule encoders. */
+    /** Zeroes all the SwerveModule encoders. */
-  public void resetEncoders() {
+    public void resetEncoders() {
-    m_drive.setPosition(0);
+        m_drive.setPosition(0);
-  }
+    }
 }

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

@@ -4,9 +4,8 @@ import edu.wpi.first.networktables.BooleanSubscriber;
 import edu.wpi.first.networktables.DoubleSubscriber;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
-public class Vision extends SubsystemBase{
+public class TylerVision{
 
     private DoubleSubscriber cam1GlobalPose;
     private DoubleSubscriber cam1ClosestTag;
@@ -14,7 +13,7 @@ public class Vision extends SubsystemBase{
 
     private DoubleSubscriber framerate;
 
-    public Vision(){
+    public TylerVision(){
         NetworkTableInstance inst = NetworkTableInstance.getDefault();
 
         NetworkTable visionTable = inst.getTable("Fiducial");