Merge branch 'master' into pre-vaale

Author: SirBeans

src/main/kotlin/com/team4099/robot2026/RobotContainer.kt

@@ -52,6 +52,8 @@ import com.team4099.robot2026.util.driver.Jessika
 import edu.wpi.first.wpilibj.RobotBase
 import edu.wpi.first.wpilibj2.command.ConditionalCommand
 import edu.wpi.first.wpilibj2.command.InstantCommand
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup
+import edu.wpi.first.wpilibj2.command.WaitCommand
 import org.ironmaple.simulation.SimulatedArena
 import org.ironmaple.simulation.drivesims.SwerveDriveSimulation
 import org.ironmaple.simulation.seasonspecific.rebuilt2026.Arena2026Rebuilt
@@ -216,7 +218,15 @@ object RobotContainer {
     ControlBoard.prepClimb.onTrue(superstructure.requestPrepClimbCommand())
     ControlBoard.climb.onTrue(superstructure.requestClimbCommand())
 
-    ControlBoard.intake.onTrue(superstructure.requestIntakeCommand())
+    ControlBoard.intake.onTrue(
+        ConditionalCommand(
+            SequentialCommandGroup(
+                superstructure.runOnce { superstructure.jigglingIntake = true },
+                WaitCommand(0.5),
+                superstructure.runOnce { superstructure.jigglingIntake = false }),
+            superstructure.requestIntakeCommand()) {
+              superstructure.currentState == Superstructure.Companion.SuperstructureStates.INTAKE
+            })
     ControlBoard.forceIntakeFullUp.whileTrue(
         superstructure.requestForceIntakeCommand(IntakeConstants.ANGLES.FORCE_UP_ANGLE))
     ControlBoard.forceIntakeHalfUp.whileTrue(

src/main/kotlin/com/team4099/robot2026/config/constants/IntakeConstants.kt

@@ -68,6 +68,7 @@ object IntakeConstants {
     val FORCE_DOWN_ANGLE = -30.degrees
 
     val RESET_INTAKE_ANGLE = INTAKE_ANGLE - 3.degrees
+    val INTAKING_JIGGLE_ANGLE = INTAKE_ANGLE + 40.degrees
   }
 
   object PID {
@@ -76,7 +77,7 @@ object IntakeConstants {
     val REAL_PIVOT_KI: IntegralGain<Radian, Volt> = 0.0.volts / (1.0.radians * 1.0.seconds)
     val REAL_PIVOT_KD: DerivativeGain<Radian, Volt> = 0.0.volts / 1.0.radians.perSecond
 
-    val SIM_PIVOT_KP: ProportionalGain<Radian, Volt> = .1.volts / 1.0.radians
+    val SIM_PIVOT_KP: ProportionalGain<Radian, Volt> = 5.volts / 1.0.radians
     val SIM_PIVOT_KI: IntegralGain<Radian, Volt> = 0.0.volts / (1.0.radians * 1.0.seconds)
     val SIM_PIVOT_KD: DerivativeGain<Radian, Volt> = 0.0.volts / 1.0.radians.perSecond
 

src/main/kotlin/com/team4099/robot2026/subsystems/superstructure/Superstructure.kt

@@ -61,6 +61,8 @@ class Superstructure(
 
   val field = Field2d()
 
+  var jigglingIntake = false
+
   init {
     SmartDashboard.putData("Field", field)
   }
@@ -244,7 +246,9 @@ class Superstructure(
         intakeRollers.currentRequest =
             Request.RollersRequest.OpenLoop(RollersConstants.INTAKE_VOLTAGE)
         intake.currentRequest =
-            Request.IntakeRequest.TargetingPosition(IntakeConstants.ANGLES.INTAKE_ANGLE)
+            Request.IntakeRequest.TargetingPosition(
+                if (jigglingIntake) IntakeConstants.ANGLES.INTAKING_JIGGLE_ANGLE
+                else IntakeConstants.ANGLES.INTAKE_ANGLE)
 
         when (currentRequest) {
           is SuperstructureRequest.Idle -> nextState = SuperstructureStates.IDLE