Changes from build session 3/3/26

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

@@ -0,0 +1,31 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Start to outpost"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Outpost to Ranged Shot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Auto Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/Outpost to Ranged Shot.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.7089624329216013,
+        "y": 0.668228980317108
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.1043470483005366,
+        "y": 0.7169051878354196
+      },
+      "isLocked": false,
+      "linkedName": "Outpost"
+    },
+    {
+      "anchor": {
+        "x": 2.3639534883720925,
+        "y": 2.9722361359570666
+      },
+      "prevControl": {
+        "x": 2.234150268336314,
+        "y": 2.5666010733452596
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 24.304549265936604
+  },
+  "reversed": false,
+  "folder": "Right Outpost",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Start to outpost.path

@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6619856887266913,
+        "y": 2.2583184257605784
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.4937567084046877,
+        "y": 1.3172450805011864
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.7089624329216013,
+        "y": 0.668228980317108
+      },
+      "prevControl": {
+        "x": 1.6987119856944102,
+        "y": 1.0414132379199703
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Outpost"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.7234468937875753,
+      "rotationDegrees": 180.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Intake Start",
+      "waypointRelativePos": 0.4500000000000002,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "Right Outpost",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/settings.json

@@ -2,7 +2,9 @@
   "robotWidth": 0.921,
   "robotLength": 0.787,
   "holonomicMode": true,
-  "pathFolders": [],
+  "pathFolders": [
+    "Right Outpost"
+  ],
   "autoFolders": [],
   "defaultMaxVel": 3.0,
   "defaultMaxAccel": 3.0,

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

@@ -42,7 +42,7 @@ import frc.robot.utilities.Elastic;
 import frc.robot.utilities.Utilities;
 
 public class RobotContainer {
-    //private PhotonVision vision;
+    private PhotonVision vision;
     private Drivetrain drivetrain;
     private Hood hood;
     private Shooter shooter;
@@ -59,7 +59,7 @@ public class RobotContainer {
     private Timer shiftTimer;
 
     public RobotContainer() {
-        //vision = new PhotonVision();
+        vision = new PhotonVision();
         drivetrain = new Drivetrain(null);
         hood = new Hood();
         shooter = new Shooter();
@@ -68,14 +68,14 @@ public class RobotContainer {
         spindexer = new Spindexer();
         //climber = new Climber();
 
-        /*
+        
         vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
         vision.addPoseEstimateConsumer((vp) -> {
             Logger.recordOutput(
                 "Vision/" + vp.cameraName() + "/Pose", 
                 vp.visualPose()
             );
-        });*/
+        });
 
         driver = new CommandXboxController(OIConstants.kDriverControllerPort);
         secondary = new CommandXboxController(OIConstants.kOperatorControllerPort);
@@ -108,8 +108,8 @@ public class RobotContainer {
         // This should just work, if it doesn't it's likely modules aren't assigned the right IDs
         // after the electronics rebuild. For testing normal operation nothing about the Drivetrain
         // class should need to change
-        drivetrain.setDefaultCommand(drivetrain.drive(() -> 0, () -> 0, () -> 0, () -> true));
+        //drivetrain.setDefaultCommand(drivetrain.drive(() -> 0, () -> 0, () -> 0, () -> true));
-        /*drivetrain.setDefaultCommand(
+        drivetrain.setDefaultCommand(
             drivetrain.drive(
                 driver::getLeftY,
                 driver::getLeftX, 
@@ -118,6 +118,8 @@ public class RobotContainer {
             )
         );
 
+        driver.y().whileTrue(drivetrain.zeroHeading());
+/* 
         // This needs to be tested after a prolonged amount of driving around <i>aggressively</i>.
         // Do things like going over the bump repeatedly, spin around a bunch, etc. 
         // If this works well over time, then this is likely all we need
@@ -168,7 +170,6 @@ public class RobotContainer {
         // DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted from the 
         // constants file for the subsystem having the problem
         driver.rightBumper().whileTrue(
-            //intakeRoller.runIn().alongWith(spindexer.spinToShooter())
             spindexer.spinToShooter()  
         );
 
@@ -178,7 +179,8 @@ public class RobotContainer {
         // DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted from the 
         // constants file for the subsystem having the problem
         driver.leftBumper().whileTrue(
-            intakeRoller.runOut().alongWith(spindexer.spinToIntake())
+            intakeRoller.runIn()
+            //intakeRoller.runOut().alongWith(spindexer.spinToIntake())
         );
 
         // While holding D-Pad up on the secondary controller, the shooter should spin
@@ -226,6 +228,10 @@ public class RobotContainer {
         secondary.x().whileTrue(intakePivot.maintainPosition(IntakePivotPosition.kDown));
         secondary.y().whileTrue(intakePivot.maintainPosition(IntakePivotPosition.kUp));
 
+        secondary.povUp().whileTrue(hood.trackToAngle(() -> Math.toRadians(40.0)));
+        secondary.povDown().whileTrue(hood.trackToAngle(() -> Math.toRadians(0.0)));
+        
+
         // TODO Some means of testing hood PIDF
         // TODO Some means of testing climber PIDF
     }

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

@@ -6,6 +6,7 @@ import java.io.FileReader;
 import java.io.IOException;
 import java.nio.file.Path;
 
+import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.FeedbackSensor;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
@@ -17,16 +18,17 @@ public class HoodConstants {
     // TODO Real Values
     public static final int kMotorCANID = 12;
 
-    public static final double kConversionFactor = 3.0*147.0/8.0;
+    public static final double kConversionFactor = (1.0/3.0)*(8.0/147.0)*2*Math.PI;
 
-    public static final double kP = 0;
+    public static final double kP = 1.75;
     public static final double kI = 0;
     public static final double kD = 0;
-    public static final double kS = 0;
+    public static final double kS = 0.435;
     public static final double kV = 0;
     public static final double kA = 0;
-    public static final double kStartupAngle = 0;
+    public static final double kStartupAngle = 0.0;
-    public static final double kMaxManualSpeedMultiplier = 1;
+    public static final double kMaxManualSpeedMultiplier = 0.1;
+    public static final double kTolerance = Math.toRadians(0.5);
 
     public static final double kAmpsToTriggerPositionReset = 10;
 
@@ -36,7 +38,7 @@ public class HoodConstants {
 
     public static final int kCurrentLimit = 15;
 
-    public static final boolean kInverted = false;
+    public static final boolean kInverted = true;
     public static final boolean kUseInterpolatorForAngle = false;
 
     public static final IdleMode kIdleMode = IdleMode.kBrake;
@@ -60,6 +62,7 @@ public class HoodConstants {
             .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
             .pid(kP, kI, kD)
             .outputRange(-1, 1)
+            .allowedClosedLoopError(kTolerance, ClosedLoopSlot.kSlot0)
             .feedForward
                 .sva(kS, kV, kA);
 

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

@@ -7,6 +7,7 @@ import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.FeedbackSensor;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
@@ -63,10 +64,12 @@ public class ModuleConstants {
     public static final double kTurningMotorReduction = 150.0/7.0;
     public static final double kTurningFactor = 2 * Math.PI / kTurningMotorReduction;
     // TODO Adjust? Let over from 2025
-    public static final double kTurnP = 1;
+    public static final double kTurnP = 12;
     public static final double kTurnI = 0;
     public static final double kTurnD = 0;
 
+    public static final double kTurnTolerance = Math.toRadians(0.25);
+
     public static final boolean kIsEncoderInverted = false;
 
     // TODO How sensitive is too sensitive?
@@ -118,6 +121,7 @@ public class ModuleConstants {
             .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
             .pid(kTurnP, kTurnI, kTurnD)
             .outputRange(-1, 1)
+            .allowedClosedLoopError(kTurnTolerance, ClosedLoopSlot.kSlot0)
             .positionWrappingEnabled(true)
             .positionWrappingInputRange(0, 2 * Math.PI);
             

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

@@ -2,7 +2,9 @@ package frc.robot.constants;
 
 import java.util.List;
 
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.util.Units;
 import frc.robot.utilities.PhotonVisionConfig;
 
 public class PhotonConstants {
@@ -11,7 +13,16 @@ public class PhotonConstants {
 
     // TODO Need actual values for all of this
     public static final Transform3d kCamera1RobotToCam = new Transform3d();
-    public static final Transform3d kCamera2RobotToCam = new Transform3d();
+    public static final Transform3d kCamera2RobotToCam = new Transform3d(
+        Units.inchesToMeters(1.5),
+        Units.inchesToMeters(-10.5),
+        Units.inchesToMeters(28.5),
+        new Rotation3d(
+            Units.degreesToRadians(0), 
+            Units.degreesToRadians(24), 
+            Units.degreesToRadians(-10)
+        )
+    );
 
     public static final double kCamera1HeightMeters = 0;
     public static final double kCamera1PitchRadians = 0;
@@ -21,7 +32,7 @@ public class PhotonConstants {
     // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
 
     public static final List<PhotonVisionConfig> configs = List.of(
-        new PhotonVisionConfig(kCamera1Name, kCamera1RobotToCam, kCamera1HeightMeters, kCamera1PitchRadians),
+        //new PhotonVisionConfig(kCamera1Name, kCamera1RobotToCam, kCamera1HeightMeters, kCamera1PitchRadians),
         new PhotonVisionConfig(kCamera2Name, kCamera2RobotToCam, kCamera2HeightMeters, kCamera2PitchRadians)
     );
 }

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

@@ -9,7 +9,7 @@ import edu.wpi.first.math.util.Units;
 public class ShooterConstants {
     public enum ShooterSpeeds {
         kHubSpeed(3000.0),
-        kFeedSpeed(3000.0);
+        kFeedSpeed(5000.0);
 
         private double speedMPS;
         private double speedRPM;
@@ -39,14 +39,14 @@ public class ShooterConstants {
     public static final boolean kLeftShooterMotorInverted = true;
     public static final boolean kRightShooterMotorInverted = false;
 
-    public static final double kLeftP = 0.0;//0.001;
+    public static final double kLeftP = 0.001;
     public static final double kLeftI = 0;
     public static final double kLeftD = 0;
     public static final double kLeftS = 0;
-    public static final double kLeftV = 0.00121;
+    public static final double kLeftV = 0.0013;
     public static final double kLeftA = 0;
 
-    public static final double kRightP = 0.001;//0.1;
+    public static final double kRightP = 0.001;
     public static final double kRightI = 0;
     public static final double kRightD = 0.000;
     public static final double kRightS = 0;

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

@@ -50,9 +50,9 @@ public class Climber extends SubsystemBase {
     }
 
     public Command maintainPosition(ClimberPositions position) {
-        targetPosition = position;
-
         return run(() -> {
+            targetPosition = position;
+
             controller.setSetpoint(
                 position.getPositionMeters(), 
                 ControlType.kPosition
@@ -61,9 +61,9 @@ public class Climber extends SubsystemBase {
     }
 
     public Command manualSpeed(DoubleSupplier speed) {
-        targetPosition = null;
-
         return run(() -> {
+            targetPosition = null;
+
             motor.set(speed.getAsDouble());
         });
     }

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

@@ -323,6 +323,13 @@ public class Drivetrain extends SubsystemBase {
         });
     }
 
+    public Command zeroHeading() {
+        return run(() -> {
+            gyro.reset();
+            estimator.resetRotation(new Rotation2d(0));
+        });
+    }
+
     public void consumeVisualPose(VisualPose pose) {
         estimator.addVisionMeasurement(
             pose.visualPose(), 

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

@@ -43,6 +43,7 @@ public class Hood extends SubsystemBase {
         );   
 
         encoder = motor.getEncoder();
+        encoder.setPosition(HoodConstants.kStartupAngle);
 
         controller = motor.getClosedLoopController();
 
@@ -68,9 +69,10 @@ public class Hood extends SubsystemBase {
     @Override
     public void periodic() {
         Logger.recordOutput("Hood/OutputCurrent", motor.getOutputCurrent());
-        Logger.recordOutput("Hood/CurrentTarget", currentTargetDegrees);
+        Logger.recordOutput("Hood/CurrentTarget", Math.toDegrees(currentTargetDegrees));
-        Logger.recordOutput("Hood/CurrentAngle", encoder.getPosition());
+        Logger.recordOutput("Hood/CurrentAngle", Math.toDegrees(encoder.getPosition()));
         Logger.recordOutput("Hood/AtSetpoint", controller.isAtSetpoint());
+        Logger.recordOutput("Hood/VoltageOut", motor.getAppliedOutput()*motor.getBusVoltage());
     }
 
     public Command trackToAngle(DoubleSupplier degreeAngleSupplier) {

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

@@ -60,9 +60,9 @@ public class IntakePivot extends SubsystemBase {
     }
 
     public Command maintainPosition(IntakePivotPosition position) {
-        currentTargetPosition = position;
-
         return run(() -> {
+            currentTargetPosition = position;
+
             if(currentTargetPosition == null) {
                 leftMotor.disable();
             } else {
@@ -72,9 +72,9 @@ public class IntakePivot extends SubsystemBase {
     }
 
     public Command manualSpeed(DoubleSupplier speed) {
-        currentTargetPosition = null;
-
         return run(() -> {
+            currentTargetPosition = null;
+            
             leftMotor.set(speed.getAsDouble() * IntakePivotConstants.kMaxManualSpeedMultiplier);
         });
     }

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

@@ -80,9 +80,9 @@ public class Shooter extends SubsystemBase {
     }
 
     public Command maintainSpeed(ShooterSpeeds speeds) {
-        targetSpeeds = speeds;
-
         return run(() -> {
+            targetSpeeds = speeds;
+
             if(targetSpeeds == null) {
                 leftMotor.disable();
                 rightMotor.disable();
@@ -101,9 +101,9 @@ public class Shooter extends SubsystemBase {
     }
 
     public Command manualSpeed(DoubleSupplier speed) {
-        targetSpeeds = null;
-
         return run(() -> {
+            targetSpeeds = null;
+            
             leftMotor.set(speed.getAsDouble() * ShooterConstants.kMaxManualSpeedMultiplier);
             rightMotor.set(speed.getAsDouble() * ShooterConstants.kMaxManualSpeedMultiplier);
         });