Skip to content

Commit 6793624

Browse files
00magikarp00magikarp
authored
Merge pull request #89 from team4099/event-26vache
Event 26vache
2 parents 0772fc4 + 8a87fc5 commit 6793624

7 files changed

Lines changed: 536 additions & 731 deletions

File tree

‎src/main/deploy/choreo/IntakeQuadrantL1/ClimbFlipped.traj‎

Lines changed: 190 additions & 101 deletions
Large diffs are not rendered by default.

‎src/main/deploy/choreo/IntakeQuadrantL1/IntakeQuadrantClimb.traj‎

Lines changed: 318 additions & 593 deletions
Large diffs are not rendered by default.

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

Lines changed: 13 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -228,30 +228,28 @@ object RobotContainer {
228228
Superstructure.Companion.SuperstructureStates.SCORE_AND_INTAKE
229229
})
230230
ControlBoard.manualScore.onTrue(
231-
Commands.runOnce({
232-
superstructure.overrideShooterVelocity = !superstructure.overrideShooterVelocity
233-
}))
231+
Commands.defer(
232+
{
233+
Commands.runOnce({
234+
superstructure.overrideShooterVelocity = !superstructure.overrideShooterVelocity
235+
})
236+
},
237+
setOf(superstructure)))
234238
ControlBoard.defenseMode.onTrue(
235-
Commands.runOnce({ superstructure.defenseMode = !superstructure.defenseMode }))
239+
Commands.defer(
240+
{ Commands.runOnce({ superstructure.defenseMode = !superstructure.defenseMode }) },
241+
setOf(superstructure)))
236242

237243
// ControlBoard.prepClimb.onTrue(superstructure.requestPrepClimbCommand())
238244
// ControlBoard.climb.onTrue(superstructure.requestClimbCommand())
239245

240-
ControlBoard.intake.onTrue(
241-
ConditionalCommand(
242-
SequentialCommandGroup(
243-
superstructure.runOnce { superstructure.jigglingIntake = true },
244-
WaitCommand(0.5),
245-
superstructure.runOnce { superstructure.jigglingIntake = false }),
246-
superstructure.requestIntakeCommand()) {
247-
superstructure.currentState == Superstructure.Companion.SuperstructureStates.INTAKE
248-
})
246+
ControlBoard.intake.onTrue(superstructure.requestIntakeCommand())
249247
ControlBoard.forceIntakeFullUp.whileTrue(
250248
RepeatCommand(
251249
SequentialCommandGroup(
252250
Commands.runOnce({
253251
intakeOverridingAngle =
254-
min(IntakeConstants.PIVOT_MAX_ANGLE, intakeOverridingAngle + 10.degrees)
252+
min(IntakeConstants.PIVOT_MAX_ANGLE, intakeOverridingAngle + 20.degrees)
255253
}),
256254
Commands.defer(
257255
{ superstructure.requestForceIntakeCommand(intakeOverridingAngle) },
@@ -262,7 +260,7 @@ object RobotContainer {
262260
SequentialCommandGroup(
263261
Commands.runOnce({
264262
intakeOverridingAngle =
265-
max(IntakeConstants.PIVOT_MIN_ANGLE, intakeOverridingAngle - 10.degrees)
263+
max(IntakeConstants.PIVOT_MIN_ANGLE, intakeOverridingAngle - 20.degrees)
266264
}),
267265
Commands.defer(
268266
{ superstructure.requestForceIntakeCommand(intakeOverridingAngle) },

‎src/main/kotlin/com/team4099/robot2026/auto/mode/IntakeQuadrantL1.kt‎

Lines changed: 0 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -70,9 +70,6 @@ class IntakeQuadrantL1(
7070
val mainTraj =
7171
Choreo.loadTrajectory<SwerveSample>("IntakeQuadrantL1/IntakeQuadrantClimb.traj").get()
7272

73-
val climbFlippedTraj =
74-
Choreo.loadTrajectory<SwerveSample>("IntakeQuadrantL1/ClimbFlipped.traj").get()
75-
7673
val startingPose = Pose2d(mainTraj.getInitialPose(false).get())
7774
}
7875
}

‎src/main/kotlin/com/team4099/robot2026/config/constants/ShooterConstants.kt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -52,7 +52,7 @@ object ShooterConstants {
5252
}
5353

5454
object PID {
55-
val REAL_KP: ProportionalGain<Velocity<Radian>, Ampere> = 8.5.amps / 1.0.radians.perSecond
55+
val REAL_KP: ProportionalGain<Velocity<Radian>, Ampere> = 8.9.amps / 1.0.radians.perSecond
5656
val REAL_KI: IntegralGain<Velocity<Radian>, Ampere> =
5757
0.0.amps / (1.0.radians.perSecond * 1.0.seconds)
5858
val REAL_KD: DerivativeGain<Velocity<Radian>, Ampere> =

‎src/main/kotlin/com/team4099/robot2026/config/constants/VisionConstants.kt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -79,7 +79,7 @@ object VisionConstants {
7979

8080
// x, y, θ
8181
val singleTagStdDevs: Matrix<N4?, N1?> = VecBuilder.fill(0.05, 0.05, 1.5, 9_999.0)
82-
val multiTagStdDevs: Matrix<N4?, N1?> = VecBuilder.fill(0.01, 0.01, 0.25, 0.1)
82+
val multiTagStdDevs: Matrix<N4?, N1?> = VecBuilder.fill(0.01, 0.01, 0.05, 0.1)
8383

8484
enum class OBJECT_CLASS(val id: Int, val mapleSimType: String?) {
8585
FUEL(0, "Fuel")

‎src/main/kotlin/com/team4099/robot2026/subsystems/superstructure/shooter/Shooter.kt‎

Lines changed: 13 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -480,31 +480,27 @@ class Shooter(private val io: ShooterIO) : ControlledByStateMachine() {
480480

481481
init {
482482
distanceToShooterMap.put(1.95.meters, 29.rotations.perSecond)
483-
distanceToShooterMap.put(2.11.meters, 29.rotations.perSecond)
484-
distanceToShooterMap.put(2.35.meters, 29.rotations.perSecond)
485-
distanceToShooterMap.put(2.45.meters, 30.5.rotations.perSecond)
486-
distanceToShooterMap.put(2.7.meters, 31.5.rotations.perSecond)
487-
distanceToShooterMap.put(2.9.meters, 33.rotations.perSecond)
488-
distanceToShooterMap.put(3.meters, 44.rotations.perSecond)
489-
distanceToShooterMap.put(3.2.meters, 46.rotations.perSecond)
490-
distanceToShooterMap.put(3.5.meters, 50.rotations.perSecond)
491-
distanceToShooterMap.put(3.75.meters, 52.rotations.perSecond)
492-
distanceToShooterMap.put(3.95.meters, 53.5.rotations.perSecond)
493-
distanceToShooterMap.put(4.23.meters, 56.rotations.perSecond)
494-
distanceToShooterMap.put(4.45.meters, 58.rotations.perSecond)
495-
distanceToShooterMap.put(4.79.meters, 59.rotations.perSecond)
496-
distanceToShooterMap.put(5.meters, 61.rotations.perSecond)
497-
distanceToShooterMap.put(5.41.meters, 64.rotations.perSecond)
483+
distanceToShooterMap.put(2.17.meters, 29.rotations.perSecond)
484+
distanceToShooterMap.put(2.42.meters, 32.rotations.perSecond)
485+
distanceToShooterMap.put(2.71.meters, 37.rotations.perSecond)
486+
distanceToShooterMap.put(2.95.meters, 40.rotations.perSecond)
487+
distanceToShooterMap.put(3.09.meters, 41.rotations.perSecond)
488+
distanceToShooterMap.put(3.33.meters, 46.rotations.perSecond)
489+
distanceToShooterMap.put(3.61.meters, 47.5.rotations.perSecond)
490+
distanceToShooterMap.put(3.92.meters, 50.rotations.perSecond)
491+
distanceToShooterMap.put(4.16.meters, 52.rotations.perSecond)
492+
distanceToShooterMap.put(4.4.meters, 53.5.rotations.perSecond)
493+
distanceToShooterMap.put(4.9.meters, 56.rotations.perSecond)
498494
}
499495

500496
fun distanceToShooterRPM(distanceToTarget: Length): AngularVelocity {
501-
if (1.95.meters <= distanceToTarget && distanceToTarget <= 5.41.meters) {
497+
if (1.95.meters <= distanceToTarget && distanceToTarget <= 4.9.meters) {
502498
return distanceToShooterMap.get(distanceToTarget)
503499
}
504500
return max(
505501
ShooterConstants.VELOCITIES.MINIMUM_LAUNCH_VELOCITY,
506502
min(
507-
(11.70986 * distanceToTarget.inMeters + 4.54954).rotations.perSecond,
503+
(10.1634 * distanceToTarget.inMeters + 9.20232).rotations.perSecond,
508504
ShooterConstants.VELOCITIES.MAXIMUM_LAUNCH_VELOCITY))
509505
}
510506
}

0 commit comments

Comments
 (0)