From e6788b2c99d11fd0e5b90469c1c94c42170fca88 Mon Sep 17 00:00:00 2001 From: BlairRobotProject Date: Sat, 28 Sep 2024 07:58:09 -0400 Subject: [PATCH] pre-cri fixes --- .../robot2024/constants/drives/SwerveConstants.kt | 2 +- .../constants/subsystem/SpinShooterKrakenConstants.kt | 8 ++++---- .../robot2024/subsystems/NewControllerBindings.kt | 10 ++++++---- .../robot2024/subsystems/shooter/SpinShooterKraken.kt | 2 ++ 4 files changed, 13 insertions(+), 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 index 0547e76..8f77826 100644 --- 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 index da47712..41be7df 100644 --- 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 index f1b818f..f9f5b84 100644 --- 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 index 154c388..2cae6d7 100644 --- 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)