Skip to content

Commit dc9db3c

Browse files
00magikarp00magikarp
authored
Merge pull request #95 from team4099/tuning-dcmp
everything pre dcmp
2 parents bf59773 + 111805d commit dc9db3c

43 files changed

Lines changed: 2320 additions & 1095 deletions

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

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

Lines changed: 222 additions & 0 deletions
Large diffs are not rendered by default.

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

Lines changed: 174 additions & 191 deletions
Large diffs are not rendered by default.

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

Lines changed: 307 additions & 317 deletions
Large diffs are not rendered by default.

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

Lines changed: 298 additions & 0 deletions
Large diffs are not rendered by default.

‎src/main/deploy/choreo/IntakeQuadrantL1/IntakeQuadrantL1.chor‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -48,8 +48,8 @@
4848
"val":607.3745796940267
4949
},
5050
"tmax":{
51-
"exp":"0.55 N * m",
52-
"val":0.55
51+
"exp":"1 N * m",
52+
"val":1.0
5353
},
5454
"cof":{
5555
"exp":"1.2",

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

Lines changed: 365 additions & 0 deletions
Large diffs are not rendered by default.

‎src/main/deploy/choreo/Tuning/NewPath.traj‎

Lines changed: 210 additions & 222 deletions
Large diffs are not rendered by default.

‎src/main/deploy/choreo/Tuning/tune_big_circle.chor‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -28,8 +28,8 @@
2828
}
2929
},
3030
"mass":{
31-
"exp":"125 lbs",
32-
"val":56.69904625
31+
"exp":"135 lbs",
32+
"val":61.23496995
3333
},
3434
"inertia":{
3535
"exp":"6.3 kg m ^ 2",
@@ -48,8 +48,8 @@
4848
"val":607.3745796940267
4949
},
5050
"tmax":{
51-
"exp":"0.55 N * m",
52-
"val":0.55
51+
"exp":"1 N * m",
52+
"val":1.0
5353
},
5454
"cof":{
5555
"exp":"1.2",

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

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@ import com.team4099.robot2026.auto.AutonomousSelector
77
import com.team4099.robot2026.commands.drivetrain.DrivePathOTF
88
import com.team4099.robot2026.config.ControlBoard
99
import com.team4099.robot2026.config.constants.Constants
10+
import com.team4099.robot2026.config.constants.FieldConstants
1011
import com.team4099.robot2026.subsystems.superstructure.Request
1112
import com.team4099.robot2026.util.Alert
1213
import com.team4099.robot2026.util.Alert.AlertType
@@ -125,9 +126,10 @@ object Robot : LoggedRobot() {
125126

126127
LiveWindow.disableAllTelemetry()
127128

128-
// init robot container too
129+
// init a buncha things
129130
RobotContainer
130131
AutonomousSelector
132+
FieldConstants.fieldLayout
131133
RobotContainer.mapDefaultCommands()
132134

133135
// init commands that have long startup
@@ -239,8 +241,10 @@ object Robot : LoggedRobot() {
239241
} else {
240242
CustomLogger.recordOutput("MatchData/ScoringNow", shiftIndex % 2 != 0)
241243
}
242-
if (time > 25) {
244+
if (125 > time && time > 25) {
243245
CustomLogger.recordOutput("MatchData/ShiftTimeLeft", time % 25)
246+
} else if (time > 125) {
247+
CustomLogger.recordOutput("MatchData/ShiftTimeLeft", time % 10)
244248
} else {
245249
CustomLogger.recordOutput("MatchData/ShiftTimeLeft", time % 30)
246250
}

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

Lines changed: 113 additions & 46 deletions
Original file line numberDiff line numberDiff line change
@@ -2,10 +2,10 @@ package com.team4099.robot2026
22

33
import com.ctre.phoenix6.signals.NeutralModeValue
44
import com.team4099.robot2026.auto.AutonomousSelector
5+
import com.team4099.robot2026.commands.AgitateIntakeCommand
56
import com.team4099.robot2026.commands.drivetrain.AimOTFCommand
67
import com.team4099.robot2026.commands.drivetrain.DrivePathOTF
78
import com.team4099.robot2026.commands.drivetrain.ResetGyroYawCommand
8-
import com.team4099.robot2026.commands.drivetrain.TargetAngleCommand
99
import com.team4099.robot2026.commands.drivetrain.TeleopDriveCommand
1010
import com.team4099.robot2026.config.ControlBoard
1111
import com.team4099.robot2026.config.constants.Constants
@@ -36,6 +36,7 @@ import com.team4099.robot2026.subsystems.superstructure.hopper.HopperIOTalon
3636
import com.team4099.robot2026.subsystems.superstructure.intake.Intake
3737
import com.team4099.robot2026.subsystems.superstructure.intake.IntakeIO
3838
import com.team4099.robot2026.subsystems.superstructure.intake.IntakeIOSim
39+
import com.team4099.robot2026.subsystems.superstructure.intake.IntakeIOTalon
3940
import com.team4099.robot2026.subsystems.superstructure.intake.rollers.IntakeRollers
4041
import com.team4099.robot2026.subsystems.superstructure.intake.rollers.IntakeRollersIO
4142
import com.team4099.robot2026.subsystems.superstructure.intake.rollers.IntakeRollersIOSim
@@ -47,7 +48,6 @@ import com.team4099.robot2026.subsystems.superstructure.shooter.ShooterIOTalon
4748
import com.team4099.robot2026.subsystems.vision.Vision
4849
import com.team4099.robot2026.subsystems.vision.camera.CameraIOPVSim
4950
import com.team4099.robot2026.subsystems.vision.camera.CameraIOPhotonvision
50-
import com.team4099.robot2026.util.AllianceFlipUtil
5151
import com.team4099.robot2026.util.driver.Jessika
5252
import edu.wpi.first.wpilibj.RobotBase
5353
import edu.wpi.first.wpilibj2.command.Commands
@@ -117,13 +117,14 @@ object RobotContainer {
117117
climb = Climb(object : ClimbIO {})
118118
feeder = Feeder(FeederIOTalonFX)
119119
hopper = Hopper(HopperIOTalon)
120-
intake = Intake(object : IntakeIO {})
120+
intake = Intake(IntakeIOTalon)
121121
intakeRollers = IntakeRollers(IntakeRollersIOTalon)
122122
shooter = Shooter(ShooterIOTalon)
123123
leds =
124124
Leds(
125125
{ isAligning },
126126
{ Superstructure.Companion.SuperstructureStates.UNINITALIZED },
127+
{ false },
127128
LedIOCandle(Constants.LEDS.CANDLE_ID))
128129
}
129130
Constants.WHOAMI.TESTBOT -> {
@@ -138,6 +139,7 @@ object RobotContainer {
138139
Leds(
139140
{ isAligning },
140141
{ Superstructure.Companion.SuperstructureStates.UNINITALIZED },
142+
{ false },
141143
object : LedIO {})
142144
}
143145
}
@@ -180,13 +182,15 @@ object RobotContainer {
180182
Leds(
181183
{ isAligning },
182184
{ Superstructure.Companion.SuperstructureStates.UNINITALIZED },
185+
{ false },
183186
object : LedIO {})
184187
}
185188

186189
superstructure =
187190
Superstructure(drivetrain, vision, climb, feeder, hopper, intake, intakeRollers, shooter)
188191

189192
leds.stateSupplier = { superstructure.currentState }
193+
leds.manualScoringSupplier = { superstructure.overrideShooterVelocity }
190194
}
191195

192196
fun mapDefaultCommands() {
@@ -210,8 +214,31 @@ object RobotContainer {
210214

211215
fun mapTeleopControls() {
212216
ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain))
217+
213218
ControlBoard.forceHome.onTrue(superstructure.requestForceHomeCommand())
214-
ControlBoard.unjam.onTrue(superstructure.requestUnjamCommand())
219+
ControlBoard.forceHome.onFalse(
220+
ConditionalCommand(
221+
superstructure.requestIntakeCommand(),
222+
superstructure.requestIdleCommand(),
223+
) {
224+
ControlBoard.intake.asBoolean
225+
})
226+
227+
ControlBoard.unjam.onTrue(
228+
ConditionalCommand(superstructure.requestUnjamCommand(), InstantCommand()) {
229+
superstructure.currentState == Superstructure.Companion.SuperstructureStates.PREP_SCORE ||
230+
superstructure.currentState ==
231+
Superstructure.Companion.SuperstructureStates.SCORE_AND_INTAKE ||
232+
superstructure.currentState == Superstructure.Companion.SuperstructureStates.SCORE
233+
})
234+
235+
ControlBoard.unjam.onFalse(
236+
ConditionalCommand(
237+
superstructure.requestScoreCommand(),
238+
InstantCommand(),
239+
) {
240+
superstructure.currentState == Superstructure.Companion.SuperstructureStates.UNJAM
241+
})
215242

216243
ControlBoard.forceIdle.onTrue(superstructure.requestIdleCommand())
217244

@@ -242,61 +269,90 @@ object RobotContainer {
242269
// ControlBoard.climb.onTrue(superstructure.requestClimbCommand())
243270

244271
ControlBoard.intake.onTrue(superstructure.requestIntakeCommand())
245-
ControlBoard.forceIntakeFullUp.whileTrue(
272+
ControlBoard.intake.onFalse(superstructure.requestIdleCommand())
273+
ControlBoard.forceIntakeUpTrigger.whileTrue(
246274
RepeatCommand(
247275
SequentialCommandGroup(
248276
Commands.runOnce({
249277
intakeOverridingAngle =
250-
min(IntakeConstants.PIVOT_MAX_ANGLE, intakeOverridingAngle + 20.degrees)
278+
min(
279+
IntakeConstants.PIVOT_MAX_ANGLE - 20.degrees,
280+
intakeOverridingAngle + 40.degrees)
251281
}),
252282
Commands.defer(
253283
{ superstructure.requestForceIntakeCommand(intakeOverridingAngle) },
254284
setOf(superstructure)),
255285
WaitCommand(0.1))))
256-
ControlBoard.forceIntakeFullDown.whileTrue(
286+
ControlBoard.forceIntakeUpShoulder.whileTrue(
287+
RepeatCommand(
288+
SequentialCommandGroup(
289+
Commands.runOnce({
290+
intakeOverridingAngle = IntakeConstants.PIVOT_MAX_ANGLE - 15.degrees
291+
}),
292+
Commands.defer(
293+
{ superstructure.requestForceIntakeCommand(intakeOverridingAngle) },
294+
setOf(superstructure)),
295+
WaitCommand(0.1))))
296+
297+
ControlBoard.forceIntakeDownTrigger.whileTrue(
257298
RepeatCommand(
258299
SequentialCommandGroup(
259300
Commands.runOnce({
260301
intakeOverridingAngle =
261-
max(IntakeConstants.PIVOT_MIN_ANGLE, intakeOverridingAngle - 20.degrees)
302+
max(
303+
IntakeConstants.PIVOT_MIN_ANGLE + 15.degrees,
304+
intakeOverridingAngle - 40.degrees)
262305
}),
263306
Commands.defer(
264307
{ superstructure.requestForceIntakeCommand(intakeOverridingAngle) },
265308
setOf(superstructure)),
266309
WaitCommand(0.1))))
267310

268-
ControlBoard.rotateTrench.whileTrue(
269-
TargetAngleCommand(
270-
Jessika(),
271-
{ ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
272-
{ ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
273-
{ ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
274-
{ ControlBoard.slowMode },
275-
drivetrain,
276-
{
277-
if (FieldConstants.inTrenchAllianceZone(drivetrain.pose) &&
278-
!AllianceFlipUtil.shouldFlip() ||
279-
!FieldConstants.inTrenchAllianceZone(drivetrain.pose) &&
280-
AllianceFlipUtil.shouldFlip())
281-
0.degrees
282-
else 180.degrees
283-
}))
284-
ControlBoard.rotateBump.whileTrue(
285-
TargetAngleCommand(
286-
Jessika(),
287-
{ ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
288-
{ ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
289-
{ ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
290-
{ ControlBoard.slowMode },
291-
drivetrain,
292-
{
293-
if (FieldConstants.inTrenchAllianceZone(drivetrain.pose) &&
294-
!AllianceFlipUtil.shouldFlip() ||
295-
!FieldConstants.inTrenchAllianceZone(drivetrain.pose) &&
296-
AllianceFlipUtil.shouldFlip())
297-
45.degrees
298-
else 225.degrees
299-
}))
311+
ControlBoard.forceIntakeDownShoulder.whileTrue(
312+
RepeatCommand(
313+
SequentialCommandGroup(
314+
Commands.runOnce({
315+
intakeOverridingAngle = IntakeConstants.PIVOT_MIN_ANGLE + 5.degrees
316+
}),
317+
Commands.defer(
318+
{ superstructure.requestForceIntakeCommand(intakeOverridingAngle) },
319+
setOf(superstructure)),
320+
WaitCommand(0.1))))
321+
322+
ControlBoard.jiggle.whileTrue(AgitateIntakeCommand(superstructure, intake))
323+
324+
// ControlBoard.rotateTrench.whileTrue(
325+
// TargetAngleCommand(
326+
// Jessika(),
327+
// { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
328+
// { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
329+
// { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
330+
// { ControlBoard.slowMode },
331+
// drivetrain,
332+
// {
333+
// if (FieldConstants.inTrenchAllianceZone(drivetrain.pose) &&
334+
// !AllianceFlipUtil.shouldFlip() ||
335+
// !FieldConstants.inTrenchAllianceZone(drivetrain.pose) &&
336+
// AllianceFlipUtil.shouldFlip())
337+
// 0.degrees
338+
// else 180.degrees
339+
// }))
340+
// ControlBoard.rotateBump.whileTrue(
341+
// TargetAngleCommand(
342+
// Jessika(),
343+
// { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
344+
// { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
345+
// { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
346+
// { ControlBoard.slowMode },
347+
// drivetrain,
348+
// {
349+
// if (FieldConstants.inTrenchAllianceZone(drivetrain.pose) &&
350+
// !AllianceFlipUtil.shouldFlip() ||
351+
// !FieldConstants.inTrenchAllianceZone(drivetrain.pose) &&
352+
// AllianceFlipUtil.shouldFlip())
353+
// 45.degrees
354+
// else 225.degrees
355+
// }))
300356

301357
ControlBoard.score.whileTrue(
302358
ConditionalCommand(
@@ -328,11 +384,21 @@ object RobotContainer {
328384
FieldConstants.inTrenchAllianceZone(drivetrain.pose)
329385
})
330386

331-
ControlBoard.climbOTF.whileTrue(
332-
ConditionalCommand(
333-
DrivePathOTF.alignClimbBottom(drivetrain), DrivePathOTF.alignClimbTop(drivetrain)) {
334-
FieldConstants.inClimbLowerHalf(drivetrain.pose)
335-
})
387+
// ControlBoard.climbOTF.whileTrue(
388+
// ConditionalCommand(
389+
// DrivePathOTF.alignClimbBottom(drivetrain), DrivePathOTF.alignClimbTop(drivetrain))
390+
// {
391+
// FieldConstants.inClimbLowerHalf(drivetrain.pose)
392+
// })\][
393+
394+
// ControlBoard.quasiForward.whileTrue(
395+
// drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kForward))
396+
// ControlBoard.quasiBackward.whileTrue(
397+
// drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kReverse))
398+
//
399+
// ControlBoard.dynamicForward.whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kForward))
400+
//
401+
// ControlBoard.dynamicBackward.whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kReverse))
336402

337403
ControlBoard.eject.onTrue(superstructure.requestEjectCommand())
338404
}
@@ -341,7 +407,8 @@ object RobotContainer {
341407

342408
fun mapTunableCommands() {}
343409

344-
fun getAutonomousCommand() = AutonomousSelector.getCommand(drivetrain, vision, superstructure)
410+
fun getAutonomousCommand() =
411+
AutonomousSelector.getCommand(drivetrain, vision, superstructure, intake)
345412

346413
fun resetSimulationField() {
347414
if (!RobotBase.isSimulation()) return

0 commit comments

Comments
 (0)