pre-cri fixes

blair-robot-project · Sep 28, 2024 · e6788b2 · e6788b2
1 parent c1d626e
commit e6788b2
Show file tree

Hide file tree

Showing 4 changed files with 13 additions and 9 deletions.
diff --git a/src/main/kotlin/frc/team449/robot2024/constants/drives/SwerveConstants.kt b/src/main/kotlin/frc/team449/robot2024/constants/drives/SwerveConstants.kt
@@ -41,7 +41,7 @@ object SwerveConstants {
   const val STEER_KS = 0.05 / 12.0
 
   /** PID gains for driving each module*/
-  const val DRIVE_KP = 0.475
+  const val DRIVE_KP = 0.95
   const val DRIVE_KI = 0.0
   const val DRIVE_KD = 0.0
 

diff --git a/src/main/kotlin/frc/team449/robot2024/constants/subsystem/SpinShooterKrakenConstants.kt b/src/main/kotlin/frc/team449/robot2024/constants/subsystem/SpinShooterKrakenConstants.kt
@@ -55,12 +55,12 @@ object SpinShooterKrakenConstants {
   const val RIGHT_KV = 0.062757 // 0.010836
   const val LEFT_KA = 0.014748 // 0.0061217
   const val RIGHT_KA = 0.015507 // 0.00815
-  const val LEFT_KP = 0.15
-  const val RIGHT_KP = 0.175
+  const val LEFT_KP = 0.1
+  const val RIGHT_KP = 0.125
   const val LEFT_KI = 0.0
   const val RIGHT_KI = 0.0
-  const val LEFT_KD = 0.1
-  const val RIGHT_KD = 0.1
+  const val LEFT_KD = 0.0
+  const val RIGHT_KD = 0.0
 
   const val MAX_AUTO_AIM_DRIVE_SPEED = 0.10
 

diff --git a/src/main/kotlin/frc/team449/robot2024/subsystems/NewControllerBindings.kt b/src/main/kotlin/frc/team449/robot2024/subsystems/NewControllerBindings.kt
@@ -313,8 +313,9 @@ class NewControllerBindings(
         SequentialCommandGroup(
           checkNoteInLocation(),
           ParallelCommandGroup(
-            robot.undertaker.slowIntake().andThen(
-              WaitUntilCommand { robot.shooter.atAmpSetpoint() && robot.pivot.inAmpTolerance() },
+            robot.undertaker.slowIntake(),
+            SequentialCommandGroup(
+              WaitCommand(1.0),
               robot.feeder.intake()
             ),
             SequentialCommandGroup(
@@ -333,8 +334,9 @@ class NewControllerBindings(
         SequentialCommandGroup(
           checkNoteInLocation(),
           ParallelCommandGroup(
-            robot.undertaker.slowIntake().andThen(
-              WaitUntilCommand { robot.shooter.atAmpSetpoint() && robot.pivot.inAmpTolerance() },
+            robot.undertaker.slowIntake(),
+            SequentialCommandGroup(
+              WaitCommand(1.0),
               robot.feeder.intake()
             ),
             robot.pivot.moveAmp(),

diff --git a/src/main/kotlin/frc/team449/robot2024/subsystems/shooter/SpinShooterKraken.kt b/src/main/kotlin/frc/team449/robot2024/subsystems/shooter/SpinShooterKraken.kt
@@ -573,10 +573,12 @@ open class SpinShooterKraken(
     builder.addDoubleProperty("2.2 Right Current Speed", { rightVelocity.get() }, null)
     builder.addDoubleProperty("2.3 Left Desired Speed", { desiredVels.first }, null)
     builder.addDoubleProperty("2.4 Right Desired Speed", { desiredVels.second }, null)
+    builder.addDoubleProperty("2.5 Left Desired Speed", { leftMotor.closedLoopReference.valueAsDouble }, null)
     builder.publishConstString("3.0", "Velocity Errors")
     builder.addDoubleProperty("3.3 Left Vel Error", { leftMotor.closedLoopError.value }, null)
     builder.addDoubleProperty("3.4 Right Vel Error", { rightMotor.closedLoopError.value }, null)
     builder.addBooleanProperty("3.5 In tolerance", ::atAimSetpoint, null)
+    builder.addBooleanProperty("3.6 In amp tolerance", ::atAmpSetpoint, null)
     builder.publishConstString("4.0", "Shoot from Anywhere")
     builder.addDoubleProperty("4.1 Speaker Distance (meters)", { abs(FieldConstants.SPEAKER_POSE.getDistance(robot.drive.pose.translation)) }, null)
     builder.addBooleanProperty("4.2 Drive In Angle Tol", { robot.driveCommand.checkSnapToAngleTolerance() }, null)