@@ -124,10 +124,16 @@ public RobotContainer() {
124
124
125
125
private void configureButtonBindings () {
126
126
Command manualPivotCommand = new ManualPivotCommand (shooter , copilot );
127
+ // coop:button(LJoystick,DT Translate,pilot)
128
+ // coop:button(RJoystick,DT Rotate,pilot)
127
129
drivetrain .setDefaultCommand (new SwerveDriveCommand (drivetrain , driver ));
130
+ // coop:button(RJoystick,Elevator Manual,copilot)
128
131
elevator .setDefaultCommand (new ElevatorManualCommand (elevator , copilot ));
129
132
shooter .setDefaultCommand (manualPivotCommand );
133
+
134
+ // coop:button(B,X-Lock,pilot)
130
135
driver .b ().onTrue (Commands .runOnce (drivetrain ::stopWithX , drivetrain ));
136
+ // coop:button(Y,Reorient,pilot)
131
137
driver .y ().onTrue (Commands .runOnce (() -> {
132
138
drivetrain .zeroFieldOrientationManual ();
133
139
enableBrakeMode (false );
@@ -154,11 +160,14 @@ private void configureButtonBindings() {
154
160
ampLock .cancel ();
155
161
counterShuffleDrive .cancel ();
156
162
});
163
+ //coop:button(X,Shoot,pilot)
157
164
driver .x ()
158
165
.whileTrue (IntakeAndFeed .withDefaults (indexer ))
159
166
.onFalse (cancelAlignment );
160
167
168
+ // coop:button(RBumper,[TOGGLE] Align Shoot,pilot)
161
169
driver .rightBumper ().toggleOnTrue (targetDrive );
170
+ // coop:button(LBumper,[TOGGLE] Align Shuffle,pilot)
162
171
driver .leftBumper ().toggleOnTrue (overstageTargetDrive );
163
172
164
173
// TODO remove this
@@ -186,81 +195,84 @@ private void configureButtonBindings() {
186
195
187
196
// driver.povDown().and(() -> !DriverStation.isFMSAttached()).onTrue(Commands.runOnce(() -> drivetrain.setPose(new Pose2d(Units.inchesToMeters(260), Units.inchesToMeters(161.62), Rotation2d.fromRadians(0)))).ignoringDisable(true));
188
197
198
+ // coop:button(RTrigger,Countershuffle,pilot)
189
199
driver .leftTrigger (0.95 ).toggleOnTrue (counterShuffleDrive );
190
200
191
201
driver .rightStick ().onTrue (cancelAlignment );
192
202
193
203
194
- driver .povLeft ().whileTrue (
195
- new DriveWithTargetingCommand (drivetrain , driver .getHID (),
196
- ()->drivetrain .getPose ().plus (
197
- new Transform2d (
198
- new Translation2d ( 1 ,
199
- ((DriverStation .getAlliance ().orElse (null ) == DriverStation .Alliance .Blue )
200
- ? AprilTagsCrescendo .getInstance ().getTag (AprilTagsCrescendo .Tags .CLIMB_AMP )
201
- : AprilTagsCrescendo .getInstance ().getTag (AprilTagsCrescendo .Tags .CLIMB_SOURCE ).rotateBy (new Rotation3d (0 , 0 , Math .PI )))
202
- .getRotation ().toRotation2d ()
203
- ), new Rotation2d ()
204
- )
205
- )
206
- )
207
- );
208
- driver .povRight ().whileTrue (
209
- new DriveWithTargetingCommand (drivetrain , driver .getHID (),
210
- ()->drivetrain .getPose ().plus (
211
- new Transform2d (
212
- new Translation2d ( 1 ,
213
- ((DriverStation .getAlliance ().orElse (null ) == DriverStation .Alliance .Blue )
214
- ? AprilTagsCrescendo .getInstance ().getTag (AprilTagsCrescendo .Tags .CLIMB_SOURCE )
215
- : AprilTagsCrescendo .getInstance ().getTag (AprilTagsCrescendo .Tags .CLIMB_AMP ).rotateBy (new Rotation3d (0 , 0 , Math .PI )))
216
- .getRotation ().toRotation2d ()
217
- ), new Rotation2d ()
218
- )
219
- )
220
- )
221
- );
222
- driver .povUp ().whileTrue (
223
- new DriveWithTargetingCommand (drivetrain , driver .getHID (),
224
- ()->drivetrain .getPose ().plus (
225
- new Transform2d (
226
- new Translation2d (1 ,
227
- FlipUtil .flipIfRed (AprilTagsCrescendo .getInstance ().getTag (AprilTagsCrescendo .Tags .CLIMB_FAR ).getRotation ().toRotation2d ())
228
- ), new Rotation2d ()
229
- )
230
- )
231
- )
232
- );
233
-
204
+ // driver.povLeft().whileTrue(
205
+ // new DriveWithTargetingCommand(drivetrain, driver.getHID(),
206
+ // ()->drivetrain.getPose().plus(
207
+ // new Transform2d(
208
+ // new Translation2d( 1,
209
+ // ((DriverStation.getAlliance().orElse(null) == DriverStation.Alliance.Blue)
210
+ // ? AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.CLIMB_AMP)
211
+ // : AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.CLIMB_SOURCE).rotateBy(new Rotation3d(0, 0, Math.PI)))
212
+ // .getRotation().toRotation2d()
213
+ // ), new Rotation2d()
214
+ // )
215
+ // )
216
+ // )
217
+ // );
218
+ // driver.povRight().whileTrue(
219
+ // new DriveWithTargetingCommand(drivetrain, driver.getHID(),
220
+ // ()->drivetrain.getPose().plus(
221
+ // new Transform2d(
222
+ // new Translation2d( 1,
223
+ // ((DriverStation.getAlliance().orElse(null) == DriverStation.Alliance.Blue)
224
+ // ? AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.CLIMB_SOURCE)
225
+ // : AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.CLIMB_AMP).rotateBy(new Rotation3d(0, 0, Math.PI)))
226
+ // .getRotation().toRotation2d()
227
+ // ), new Rotation2d()
228
+ // )
229
+ // )
230
+ // )
231
+ // );
232
+ // driver.povUp().whileTrue(
233
+ // new DriveWithTargetingCommand(drivetrain, driver.getHID(),
234
+ // ()->drivetrain.getPose().plus(
235
+ // new Transform2d(
236
+ // new Translation2d(1,
237
+ // FlipUtil.flipIfRed(AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.CLIMB_FAR).getRotation().toRotation2d())
238
+ // ), new Rotation2d()
239
+ // )
240
+ // )
241
+ // )
242
+ // );
243
+
244
+ // coop:button(Back,Zero Shooter,copilot)
234
245
copilot .back ().onTrue (Commands .runOnce (shooter ::zeroPivotToCancoder ));
235
246
247
+ // coop:button(LBumper,Score Amp,copilot)
236
248
copilot .leftBumper ().whileTrue (new AmpScoreSequence (tramp , indexer , elevator ));
237
249
Command intakeCommand = new ContinuousIntakeCommand (indexer , 1 )
238
250
.deadlineWith (CommandUtils .rumbleCommand (driver , 0.5 ), CommandUtils .rumbleCommand (copilot , 0.5 ));
251
+ // coop:button(RBumper,Intake,copilot)
239
252
copilot .rightBumper ().whileTrue (intakeCommand );
240
253
241
254
copilot .povDown ().whileTrue (indexer .commandRunIntake (-1 ));
242
255
copilot .povUp ().whileTrue (indexer .commandRunIntake (1 ));
243
256
copilot .povRight ().whileTrue (IntakeAndFeed .withDefaults (indexer )).onFalse (cancelAlignment );
244
257
copilot .povLeft ().onTrue (Commands .runOnce (()->elevator .setFlipper (true ))).onFalse (Commands .runOnce (()->elevator .setFlipper (false )));
245
- // copilot.povDown().whileTrue(new ElevatorSetpointCommand(elevator, Constants.Elevator.ElevatorState.BOTTOM));
246
- // copilot.povUp().whileTrue(new ElevatorSetpointCommand(elevator, Constants.Elevator.ElevatorState.TOP));
247
- // copilot.povRight().whileTrue(new ElevatorSetpointCommand(elevator, Constants.Elevator.ElevatorState.CLIMB));
248
- // copilot.povLeft().whileTrue(new ElevatorSetpointCommand(elevator, Constants.Elevator.ElevatorState.AMP));
249
-
250
258
259
+ // coop:button(RTrigger,Run Tramp,copilot)
251
260
copilot .rightTrigger (0.95 ).whileTrue (tramp .commandRun (1 ));
252
- copilot .leftTrigger (0.95 ).whileTrue (new ClimbAlignment (drivetrain , elevator , tramp , indexer ));
261
+ // copilot.leftTrigger(0.95).whileTrue(new ClimbAlignment(drivetrain, elevator, tramp, indexer));
253
262
254
263
255
264
copilot .x ().whileTrue (new ShootSequence (shooter , indexer ));
265
+ // coop:button(A,[HOLD] PreAmp,copilot)
256
266
copilot .a ().whileTrue (new AmpScoreStageSequence (indexer , tramp , elevator ).alongWith (ampLock ));
267
+ // coop:button(B,[HOLD] SHOOT,copilot)
257
268
copilot .b ()
258
269
.and (shooter ::areFlywheelsSpunUp )
259
270
.and (() -> targetDrive .isScheduled ()
260
271
|| overstageTargetDrive .isScheduled ()
261
272
|| counterShuffleDrive .isScheduled ())
262
273
.whileTrue (IntakeAndFeed .withDefaults (indexer ))
263
274
.onFalse (cancelAlignment );
275
+ // coop:button(Y,[HOLD] Stage Tramp,copilot)
264
276
copilot .y ().whileTrue (new StageTrampCommand (tramp , indexer ));
265
277
266
278
// copilot.leftTrigger(0.5).whileTrue(new ElevatorSetpointCommand(elevator, ElevatorState.CLIMB));
@@ -297,6 +309,7 @@ private void configureButtonBindings() {
297
309
new Trigger (tramp ::isNoteStaged ).debounce (0.1 )
298
310
.onTrue (brakeModeCommand );
299
311
312
+
300
313
driver .start ().onTrue (brakeModeCommand );
301
314
copilot .start ().onTrue (brakeModeCommand );
302
315
0 commit comments