auto getting there everything else gucci

src/main/deploy/pathplanner/autos/left to center.auto

@@ -15,6 +15,24 @@
           "data": {
             "pathName": "left start to center"
           }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "over bump to pile"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "back from center"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "auto shoot"
+          }
         }
       ]
     }

src/main/deploy/pathplanner/paths/back from center.path

@@ -0,0 +1,63 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.050842639593909,
+        "y": 5.715482233502538
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.130030456852792,
+        "y": 5.7523147208121825
+      },
+      "isLocked": false,
+      "linkedName": "over bump"
+    },
+    {
+      "anchor": {
+        "x": 3.002954314720812,
+        "y": 5.310324873096447
+      },
+      "prevControl": {
+        "x": 4.052680203045686,
+        "y": 5.9825177664974625
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.10454545454545605,
+      "rotationDegrees": -115.0
+    },
+    {
+      "waypointRelativePos": 0.7931818181818296,
+      "rotationDegrees": -115.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -36.158185439808385
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -123.34070734647689
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/left start to center.path

@@ -8,111 +8,77 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.1594923857868027,
+        "x": 2.9108730964467004,
-        "y": 6.424507614213196
+        "y": 6.498172588832488
       },
       "isLocked": false,
       "linkedName": "start left"
     },
     {
       "anchor": {
-        "x": 2.634629441624366,
+        "x": 2.5885888324873094,
-        "y": 5.558944162436549
+        "y": 5.936477157360406
       },
       "prevControl": {
-        "x": 2.4136345177664977,
+        "x": 2.3675939086294413,
-        "y": 5.908852791878173
+        "y": 6.28638578680203
       },
       "nextControl": {
-        "x": 2.895419769065523,
+        "x": 2.8493791599284664,
-        "y": 5.146026143988051
+        "y": 5.523559138911907
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 4.522294416243654,
+        "x": 4.577543147208122,
-        "y": 5.558944162436549
+        "y": 5.715482233502538
       },
       "prevControl": {
-        "x": 3.5095162657293457,
+        "x": 3.565840773084476,
-        "y": 5.533304209258972
+        "y": 5.768729726877464
       },
       "nextControl": {
-        "x": 6.704619289340102,
+        "x": 5.102406091370558,
-        "y": 5.614192893401015
+        "y": 5.687857868020306
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 7.478101522849547,
+        "x": 6.050842639593909,
-        "y": 7.244030456855094
+        "y": 5.715482233502538
       },
       "prevControl": {
-        "x": 6.765542310662505,
+        "x": 5.525979695431473,
-        "y": 7.465169522706244
+        "y": 5.807563451776649
-      },
-      "nextControl": {
-        "x": 8.012172588839395,
-        "y": 7.078284263961693
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 7.671472081218274,
-        "y": 4.7670456852791885
-      },
-      "prevControl": {
-        "x": 7.726720812182741,
-        "y": 7.639979695431472
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "over bump"
     }
   ],
   "rotationTargets": [
     {
-      "waypointRelativePos": 1.5636363636363326,
+      "waypointRelativePos": 1.62272727272727,
-      "rotationDegrees": -45.0
+      "rotationDegrees": -135.0
-    },
-    {
-      "waypointRelativePos": 3.0318181818182266,
-      "rotationDegrees": -90.0
-    }
-  ],
-  "constraintZones": [
-    {
-      "name": "Constraints Zone",
-      "minWaypointRelativePos": 2.6208291203235685,
-      "maxWaypointRelativePos": 4.0,
-      "constraints": {
-        "maxVelocity": 1.0,
-        "maxAcceleration": 1.5,
-        "maxAngularVelocity": 540.0,
-        "maxAngularAcceleration": 720.0,
-        "nominalVoltage": 12.0,
-        "unlimited": false
-      }
     }
   ],
+  "constraintZones": [],
   "pointTowardsZones": [],
   "eventMarkers": [
     {
       "name": "Intake Start",
-      "waypointRelativePos": 0,
+      "waypointRelativePos": 2.1,
       "endWaypointRelativePos": null,
       "command": null
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 2.0,
+    "maxVelocity": 3.0,
-    "maxAcceleration": 1.5,
+    "maxAcceleration": 2.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -120,7 +86,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -89.09061955080092
+    "rotation": -123.34070734647689
   },
   "reversed": false,
   "folder": null,
@@ -128,5 +94,5 @@
     "velocity": 0,
     "rotation": -90.0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

src/main/deploy/pathplanner/paths/over bump to pile.path

@@ -0,0 +1,112 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.050842639593909,
+        "y": 5.715482233502538
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.520456852791878,
+        "y": 6.406091370558377
+      },
+      "isLocked": false,
+      "linkedName": "over bump"
+    },
+    {
+      "anchor": {
+        "x": 7.017695431472081,
+        "y": 7.216406091370559
+      },
+      "prevControl": {
+        "x": 6.017695431472081,
+        "y": 7.216406091370559
+      },
+      "nextControl": {
+        "x": 8.017695431472081,
+        "y": 7.216406091370559
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.883258883248731,
+        "y": 4.831502538071066
+      },
+      "prevControl": {
+        "x": 7.941269035532995,
+        "y": 6.042370558375635
+      },
+      "nextControl": {
+        "x": 7.825248730964466,
+        "y": 3.620634517766498
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.050842639593909,
+        "y": 5.715482233502538
+      },
+      "prevControl": {
+        "x": 6.99789847715736,
+        "y": 5.183713197969544
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.0045454545454569,
+      "rotationDegrees": -115.0
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.1243680485338854,
+      "maxWaypointRelativePos": 2.0,
+      "constraints": {
+        "maxVelocity": 1.0,
+        "maxAcceleration": 1.5,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Intake Start",
+      "waypointRelativePos": 0.7886754297269942,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -120.34324888423971
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -123.34070734647689
+  },
+  "useDefaultConstraints": true
+}

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

@@ -84,7 +84,7 @@ public class RobotContainer {
 
         resetOdometryToVisualPose = false;
          
-        //vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
+        vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
         vision.addPoseEstimateConsumer((vp) -> {
             Logger.recordOutput(
                 "Vision/" + vp.cameraName() + "/Pose", 
@@ -243,7 +243,7 @@ public class RobotContainer {
 
         // Useful for testing PID and FF responses of the shooter
         // You need to have graphs up of the logged data to make sure the response is correct
-        secondary.a().whileTrue(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed));
+        //secondary.a().whileTrue(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed));
         secondary.b().whileTrue(shooter.maintainSpeed(ShooterSpeeds.kFeedSpeed));
 
         // Useful for testing PID and FF responses of the intake pivot
@@ -276,6 +276,7 @@ public class RobotContainer {
 
         driver.a().onTrue(new InstantCommand(() -> resetOdometryToVisualPose = true));
         driver.y().whileTrue(drivetrain.zeroHeading());
+        driver.x().whileTrue(drivetrain.setX());
 
         driver.leftTrigger().whileTrue(
             drivetrain.lockRotationToHub(
@@ -320,10 +321,12 @@ public class RobotContainer {
         //40 good for feeding
         //secondary.b().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(30)));
         //30 degrees good for shooter far near outpost
-        secondary.rightBumper().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(10)));
+        secondary.rightBumper().whileTrue(hood.trackToAngle(() -> Units.degreesToRadians(10)));
         //10 degrees good for shooting ~33in away from hub
 
-        hood.setDefaultCommand(hood.trackToAngle(() -> {
+        hood.setDefaultCommand(hood.trackToAnglePoseBased(drivetrain, shooter));
+        
+        /*hood.setDefaultCommand(hood.trackToAngle(() -> {
             Pose2d drivetrainPose = drivetrain.getPose();
             Pose2d hubPose = Utilities.getHubPose();
 
@@ -345,7 +348,7 @@ public class RobotContainer {
             } else {
                 return 0;
             }
-        }));
+        }));*/
 
         new Trigger(() -> MathUtil.isNear(
             shooter.getTargetSpeeds().isEmpty() ? 0 : shooter.getTargetSpeeds().get().getSpeedRPM(), 
@@ -399,7 +402,8 @@ public class RobotContainer {
        // NamedCommands.registerCommand("Intake Start", intakeRoller.runIn());
 
         new EventTrigger("Intake Start")
-            .onTrue(intakeRoller.runIn());
+            .onTrue(
+                intakeRoller.runIn());
 
         NamedCommands.registerCommand("stop spindexer", spindexer.instantaneousStop());
 
@@ -415,6 +419,8 @@ public class RobotContainer {
                                hood.trackToAngle(() -> Units.degreesToRadians(10)))
                 
             ).withTimeout(3).andThen(spindexer.instantaneousStop()));
+
+        NamedCommands.registerCommand("auto shoot", hood.trackToAnglePoseBased(drivetrain, shooter));
     }
 
     public Command getAutonomousCommand() {

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

@@ -71,22 +71,22 @@ public class HoodConstants {
 
         kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
             Double.valueOf(Units.inchesToMeters(22.2 + 40)), 
-            Double.valueOf(Units.degreesToRadians(10)));
+            Double.valueOf(Units.degreesToRadians(9.5)));
 
         kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
             Double.valueOf(Units.inchesToMeters(22.2 + 60)), 
-            Double.valueOf(Units.degreesToRadians(13)));
+            Double.valueOf(Units.degreesToRadians(12.5)));
 
         kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
             Double.valueOf(Units.inchesToMeters(22.2 + 80)),
-            Double.valueOf(Units.degreesToRadians(17)));
+            Double.valueOf(Units.degreesToRadians(16.25)));
 
         kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
             Double.valueOf(Units.inchesToMeters(22.2 + 100)), 
-            Double.valueOf(Units.degreesToRadians(21)));
+            Double.valueOf(Units.degreesToRadians(20.5)));
 
         kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
             Double.valueOf(Units.inchesToMeters(22.2 + 120)), 
-            Double.valueOf(Units.degreesToRadians(24)));
+            Double.valueOf(Units.degreesToRadians(23.5)));
     }
 }

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

@@ -5,8 +5,10 @@ import com.revrobotics.spark.FeedbackSensor;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.util.Units;
 
+
 public class ShooterConstants {
     public enum ShooterSpeeds {
         kHubSpeed(3000.0),
@@ -41,16 +43,16 @@ public class ShooterConstants {
     public static final boolean kLeftShooterMotorInverted = true;
     public static final boolean kRightShooterMotorInverted = false;
 
-    public static final double kLeftP = 0.04;//0.01;//0.001;
+    public static final double kLeftP = 0.1;//0.01;//0.001;
     public static final double kLeftI = 0;
-    public static final double kLeftD = 0;//1.8;
+    public static final double kLeftD = 0;//0.1;//1.8;
     public static final double kLeftS = 0;
     public static final double kLeftV = 0.00129;
     public static final double kLeftA = 0;
 
-    public static final double kRightP = 0.04;//0.001;//0.001;
+    public static final double kRightP = 0.1;//0.001;//0.001;
     public static final double kRightI = 0;
-    public static final double kRightD = 0;
+    public static final double kRightD = 0;//0.1;
     public static final double kRightS = 0;
     public static final double kRightV = 0.00125;
     public static final double kRightA = 0;
@@ -78,12 +80,12 @@ public class ShooterConstants {
         kLeftMotorConfig.absoluteEncoder
             .positionConversionFactor(1)
             .velocityConversionFactor(60)
-            .averageDepth(16); // VERY IMPORTANT FOR RESPONSE OF FLYWHEEL DEFAULTS ARE DOGWATER
+            .averageDepth(8); // VERY IMPORTANT FOR RESPONSE OF FLYWHEEL DEFAULTS ARE DOGWATER
         kLeftMotorConfig.closedLoop
             .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
             .pid(kLeftP, kLeftI, kLeftD, ClosedLoopSlot.kSlot0)
             .outputRange(-1, 1)
-            //.allowedClosedLoopError(10.0, ClosedLoopSlot.kSlot0)
+            .allowedClosedLoopError(25.0, ClosedLoopSlot.kSlot0)
             .feedForward
                 .sva(kLeftS, kLeftV, kLeftA, ClosedLoopSlot.kSlot0);
 
@@ -94,13 +96,13 @@ public class ShooterConstants {
         kRightMotorConfig.absoluteEncoder
             .positionConversionFactor(1)
             .velocityConversionFactor(60)
-            .averageDepth(16)// VERY IMPORTANT FOR RESPONSE OF FLYWHEEL DEFAULTS ARE DOGWATER
+            .averageDepth(8)// VERY IMPORTANT FOR RESPONSE OF FLYWHEEL DEFAULTS ARE DOGWATER
             .inverted(true);
         kRightMotorConfig.closedLoop
             .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
             .pid(kRightP, kRightI, kRightD)
             .outputRange(-1, 1)
-            //.allowedClosedLoopError(10.0, ClosedLoopSlot.kSlot0)
+            .allowedClosedLoopError(25.0, ClosedLoopSlot.kSlot0)
             .feedForward
                 .sva(kRightS, kRightV, kRightA, ClosedLoopSlot.kSlot0);
     }

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

@@ -1,5 +1,6 @@
 package frc.robot.subsystems;
 
+import java.util.Optional;
 import java.util.function.DoubleSupplier;
 
 import org.littletonrobotics.junction.Logger;
@@ -13,6 +14,8 @@ import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -20,6 +23,8 @@ import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.constants.HoodConstants;
+import frc.robot.constants.ShooterConstants.ShooterSpeeds;
+import frc.robot.utilities.Utilities;
 
 public class Hood extends SubsystemBase {
     private SparkMax motor;
@@ -92,6 +97,32 @@ public class Hood extends SubsystemBase {
         Logger.recordOutput("Hood/VoltageOut", motor.getAppliedOutput()*motor.getBusVoltage());
     }
 
+    public Command trackToAnglePoseBased(Drivetrain drivetrain, Shooter shooter) {
+        return trackToAngle(() -> {
+            Pose2d drivetrainPose = drivetrain.getPose();
+            Pose2d hubPose = Utilities.getHubPose();
+
+            double distance = drivetrainPose.getTranslation()
+                .getDistance(hubPose.getTranslation());
+
+            Logger.recordOutput("Hood/DistanceToHub", distance);
+
+            Optional<ShooterSpeeds> currentSpeeds = shooter.getTargetSpeeds();
+                
+            if(currentSpeeds.isPresent()) {
+                InterpolatingDoubleTreeMap map = HoodConstants.kHoodInterpolators.get(currentSpeeds.get());
+
+                if(map != null) {
+                    return MathUtil.clamp(map.get(distance), 0, 40);
+                } else {
+                    return 0;
+                }
+            } else {
+                return 0;
+            }
+        });
+    }
+
     public Command trackToAngle(DoubleSupplier degreeAngleSupplier) {
         return run(() -> {
             currentTargetDegrees = degreeAngleSupplier.getAsDouble();

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

@@ -9,11 +9,13 @@ import com.revrobotics.AbsoluteEncoder;
 import com.revrobotics.PersistMode;
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.ResetMode;
+import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -33,6 +35,8 @@ public class Shooter extends SubsystemBase {
     private SparkClosedLoopController rightClosedLoopController;
 
     private ShooterSpeeds targetSpeeds;
+    private SimpleMotorFeedforward shooterFFLeft;
+    private SimpleMotorFeedforward shooterFFRight;
 
     public Shooter() {
         leftMotor = new SparkMax(ShooterConstants.kLeftShooterMotorCANID, MotorType.kBrushless);
@@ -60,6 +64,9 @@ public class Shooter extends SubsystemBase {
         targetSpeeds = null;
 
         rightRelative = rightMotor.getEncoder();
+
+        shooterFFLeft = new SimpleMotorFeedforward(ShooterConstants.kLeftS, ShooterConstants.kLeftV, ShooterConstants.kLeftA);
+        shooterFFRight = new SimpleMotorFeedforward(ShooterConstants.kRightS, ShooterConstants.kRightV, ShooterConstants.kRightA);
     }
 
     @Override
@@ -116,12 +123,16 @@ public class Shooter extends SubsystemBase {
             } else {
                 leftClosedLoopController.setSetpoint(
                     targetSpeeds.getSpeedRPM(), 
-                    ControlType.kVelocity
+                    ControlType.kVelocity,
+                    ClosedLoopSlot.kSlot0,
+                    shooterFFLeft.calculate(targetSpeeds.getSpeedRPM())
                 );
 
                 rightClosedLoopController.setSetpoint(
                     targetSpeeds.getSpeedRPM(), 
-                    ControlType.kVelocity
+                    ControlType.kVelocity,
+                    ClosedLoopSlot.kSlot0,
+                    shooterFFRight.calculate(targetSpeeds.getSpeedRPM())
                 );
             }
         });