vabla code

simgui-ds.json

@@ -92,6 +92,10 @@
   "robotJoysticks": [
     {
       "guid": "Keyboard0"
+    },
+    {
+      "guid": "78696e70757401000000000000000000",
+      "useGamepad": true
     }
   ]
 }

simgui.json

@@ -57,7 +57,9 @@
       "/LiveWindow/Ungrouped/navX-Sensor[1]": "Gyro",
       "/LiveWindow/Ungrouped/navX-Sensor[4]": "Gyro",
       "/Monologuing/field": "Field2d",
+      "/Monologuing/robot/closeToShooterInfrared": "Digital Input",
       "/Monologuing/robot/elevator/mech": "Mechanism2d",
+      "/Monologuing/robot/infrared": "Digital Input",
       "/Monologuing/routineChooser": "String Chooser",
       "/Monologuing/scheduler": "Scheduler",
       "/SmartDashboard/Command Scheduler": "Scheduler",

src/main/kotlin/frc/team449/robot2024/auto/AutoUtil.kt

@@ -20,8 +20,8 @@ object AutoUtil {
         robot.feeder.intake(),
         WaitUntilCommand { !robot.infrared.get() },
         robot.undertaker.stop(),
-//        robot.feeder.outtake(),
-//        WaitUntilCommand { robot.infrared.get() },
+        robot.feeder.outtake(),
+        WaitCommand(0.10),
         robot.feeder.stop(),
       ),
       robot.shooter.shootSubwoofer()

src/main/kotlin/frc/team449/robot2024/auto/routines/Experimental3PieceMid.kt

@@ -1,5 +1,6 @@
 package frc.team449.robot2024.auto.routines
 
+import edu.wpi.first.math.controller.PIDController
 import frc.team449.control.auto.ChoreoRoutine
 import frc.team449.control.auto.ChoreoRoutineStructure
 import frc.team449.control.auto.ChoreoTrajectory
@@ -13,6 +14,9 @@ class Experimental3PieceMid(
 
   override val routine =
     ChoreoRoutine(
+      xController = PIDController(2.875, 0.0, 0.05),
+      yController = PIDController(2.875, 0.0, 0.05),
+      thetaController = PIDController(2.50, 0.0, 0.05),
       drive = robot.drive,
       parallelEventMap = hashMapOf(
         0 to AutoUtil.autoIntake(robot),

src/main/kotlin/frc/team449/robot2024/constants/drives/SwerveConstants.kt

@@ -33,9 +33,9 @@ object SwerveConstants {
   const val TURN_KD = 0.0
 
   /** Feed forward values for driving each module */
-  const val DRIVE_KS = 0.20285
-  const val DRIVE_KV = 2.3887
-  const val DRIVE_KA = 0.43365
+  const val DRIVE_KS = 0.20285 + 0.02
+  const val DRIVE_KV = 2.3887 + 0.2
+  const val DRIVE_KA = 0.43365 + 0.035
 
   // TODO: Figure out this value
   const val STEER_KS = 0.05

src/main/kotlin/frc/team449/robot2024/constants/subsystem/FeederConstants.kt

@@ -8,9 +8,9 @@ object FeederConstants {
   const val GEARING = 1.0 / 9.0
 
   const val INTAKE_VOLTAGE = 7.5
-  const val SLOW_INTAKE_VOLTAGE = 6.0
+  const val SLOW_INTAKE_VOLTAGE = 4.0
   const val AUTO_SHOOT_INTAKE_VOLTAGE = 12.0
-  const val REVERSE_VOLTAGE = -3.0
+  const val REVERSE_VOLTAGE = -2.35
 
   const val BRAKE_MODE = false
 

src/main/kotlin/frc/team449/robot2024/constants/subsystem/PivotConstants.kt

@@ -18,7 +18,7 @@ object PivotConstants {
   const val GEARING = 1.0 / 75.0
   const val UPR = 2 * PI / 1.5
   const val OFFSET = -0.2125 + (0.150882 / UPR) + (0.829237 / UPR) - (0.012295 / UPR) +
-    (0.020301 / UPR)
+    (0.020301 / UPR) + (0.339 / UPR)
   const val ENC_INVERTED = true
 
   val QUAD_ENCODER = Encoder(1, 2)
@@ -60,9 +60,9 @@ object PivotConstants {
 
   val MIN_ANGLE = Units.degreesToRadians(0.0)
   val MAX_ANGLE = Units.degreesToRadians(105.0)
-  val AMP_ANGLE = Units.degreesToRadians(100.53)
-  val CLIMB_ANGLE = Units.degreesToRadians(75.0)
-  val STOW_ANGLE = Units.degreesToRadians(-1.0)
+  val AMP_ANGLE = Units.degreesToRadians(95.0)
+  val CLIMB_ANGLE = Units.degreesToRadians(70.0)
+  val STOW_ANGLE = Units.degreesToRadians(-2.0)
 
   // IS THIS CORRECT???
   val AUTO_ANGLE = 0.350

src/main/kotlin/frc/team449/robot2024/constants/subsystem/ShooterConstants.kt

@@ -21,9 +21,9 @@ object ShooterConstants {
 
   val BRAKE_RATE_LIMIT = Units.rotationsPerMinuteToRadiansPerSecond(4000.0)
 
-  const val KS = 0.38732
+  const val KS = 0.075
   const val KV = 0.012718
-  const val KA = 0.0075191
+  const val KA = 0.010
 
   const val IN_TOLERANCE = 6.5
 

src/main/kotlin/frc/team449/robot2024/subsystems/ControllerBindings.kt

@@ -99,7 +99,13 @@ class ControllerBindings(
 
   private fun robotBindings() {
     mechanismController.y().onTrue(
-      robot.pivot.moveAmp()
+      ParallelCommandGroup(
+        robot.undertaker.slowIntake().andThen(
+          WaitCommand(0.25),
+          robot.pivot.moveAmp()
+        ),
+        robot.shooter.scoreAmp()
+      )
     )
 
     Trigger { abs(mechanismController.hid.leftY) > 0.15 || abs(mechanismController.hid.rightY) > 0.15 }.onTrue(
@@ -150,7 +156,8 @@ class ControllerBindings(
 
     mechanismController.leftBumper().onTrue(
       SequentialCommandGroup(
-        checkNoteInLocation(),
+        slowIntake(),
+        outtakeToNotePosition(),
         robot.shooter.scoreAmp(),
       )
     )
@@ -218,7 +225,8 @@ class ControllerBindings(
 
     mechanismController.rightBumper().onTrue(
       SequentialCommandGroup(
-        checkNoteInLocation(),
+        slowIntake(),
+        outtakeToNotePosition(),
         robot.shooter.shootSubwoofer(),
       )
     )

src/main/kotlin/frc/team449/robot2024/subsystems/shooter/Shooter.kt

@@ -251,8 +251,8 @@ open class Shooter(
       )
 
       val plantSim = LinearSystemId.identifyVelocitySystem(
-        ShooterConstants.KV + 0.005,
-        ShooterConstants.KA + 0.003
+        ShooterConstants.KV + 0.0125,
+        ShooterConstants.KA + 0.0065
       )
 
       val observer = KalmanFilter(