Skip to content

Commit e417e5f

Browse files
6kn4eakfr4s6kn4eakfr4s
committed
sync
1 parent 4da4032 commit e417e5f

10 files changed

+1
-950
lines changed

‎src/main/java/frc/robot/Robot.java

+1-13
Original file line numberDiff line numberDiff line change
@@ -65,19 +65,7 @@ public void disabledInit() {
6565
public void disabledPeriodic() {
6666
}
6767

68-
/**
69-
* This autonomous runs the autonomous command selected by your
70-
* {@link RobotContainer} class.
71-
*/
72-
@Override
73-
public void autonomousInit() {
74-
autonomousCommand = robotContainer.getAutonomousCommand();
75-
76-
// schedule the autonomous command (example)
77-
if (autonomousCommand != null) {
78-
autonomousCommand.schedule();
79-
}
80-
}
68+
8169

8270
/** This function is called periodically during autonomous. */
8371
@Override

‎src/main/java/frc/robot/RobotContainer.java

-289
Original file line numberDiff line numberDiff line change
@@ -10,13 +10,7 @@
1010

1111
import java.util.EnumSet;
1212

13-
import com.choreo.lib.ChoreoTrajectory;
1413
import com.fasterxml.jackson.databind.util.Named;
15-
import com.pathplanner.lib.auto.AutoBuilder;
16-
import com.pathplanner.lib.auto.NamedCommands;
17-
import com.pathplanner.lib.commands.PathPlannerAuto;
18-
import com.pathplanner.lib.path.PathPlannerPath;
19-
import com.pathplanner.lib.util.PathPlannerLogging;
2014

2115
import edu.wpi.first.cscore.MjpegServer;
2216
import edu.wpi.first.cscore.UsbCamera;
@@ -151,8 +145,6 @@ public class RobotContainer {
151145
private UsbCamera driverCamera;
152146
private MjpegServer driverCameraServer;
153147

154-
ChoreoTrajectory trajectory;
155-
156148
private PIDController xPID;
157149
private PIDController yPID;
158150

@@ -184,14 +176,6 @@ public RobotContainer() {
184176
autonTable = ntInstance.getTable("Auton");
185177
listEntry = autonTable.getEntry("AutonList");
186178
selectedAutonEntry = autonTable.getEntry("Selected");
187-
// System.out.print("Available Autons: " + AutoBuilder.getAllAutoNames().toArray(new String[0]));
188-
listEntry.setStringArray(AutoBuilder.getAllAutoNames().toArray(new String[0]));
189-
selectedAutonEntry.setString(autonValue);
190-
autonTable.addListener("Auton", EnumSet.of(NetworkTableEvent.Kind.kValueAll), (table, key, event) -> {
191-
this.autonValue = event.valueData.value.getString();
192-
this.selectedAutonEntry.setString(this.autonValue);
193-
System.out.print("New auton value: " + this.autonValue);
194-
});
195179
fmsSubsystem = new FieldManagementSubsystem();
196180
lightBarSubsystem = new LightBarSubsystem();
197181
superstructureSubsystem = new SuperstructureSubsystem(lightBarSubsystem, fmsSubsystem);
@@ -271,46 +255,6 @@ public RobotContainer() {
271255

272256
swerveCrauton.add(autonPathChooser);
273257

274-
NamedCommands.registerCommand("Shoot", new ParallelDeadlineGroup(
275-
new SequentialCommandGroup(
276-
new WaitCommand(.05),
277-
new ConditionalWaitCommand(swerveSubsystem::atTargetAngle).withTimeout(1),
278-
new IntakeRollerFeedCommand(intakeRollerSubsystem, 1).until(intakeRollerSubsystem::getRockwellSensorValue),
279-
new IntakeRollerFeedCommand(intakeRollerSubsystem, 1).until(() ->
280-
!intakeRollerSubsystem.getRockwellSensorValue()
281-
&&
282-
!intakeRollerSubsystem.getAmpSensor()
283-
&&
284-
!intakeRollerSubsystem.getFrontSensorValue()),
285-
new IntakeRollerFeedCommand(intakeRollerSubsystem, 1).withTimeout(.3)
286-
),
287-
new SwerveAimCommand(swerveSubsystem)
288-
));
289-
290-
NamedCommands.registerCommand("StationaryShoot", new ParallelDeadlineGroup(
291-
new SequentialCommandGroup(
292-
new ConditionalWaitCommand(shooterFlywheelSubsystem::atSpeed),
293-
new ConditionalWaitCommand(swerveSubsystem::atTargetAngle),
294-
new IntakeRollerFeedCommand(intakeRollerSubsystem, 1).until(() -> !intakeRollerSubsystem.getRockwellSensorValue()),
295-
new IntakeRollerFeedCommand(intakeRollerSubsystem, 1).withTimeout(.3)
296-
),
297-
new SwerveAimCommand(swerveSubsystem)
298-
));
299-
300-
NamedCommands.registerCommand(
301-
"Intake",
302-
new IntakeRollerAmpIntakeCommand(intakeRollerSubsystem).andThen(new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem))
303-
);
304-
305-
NamedCommands.registerCommand("AutoIntake",
306-
new NoteAlignCommand(swerveSubsystem, noteDetector, driveController).until(intakeRollerSubsystem::getAmpSensor).alongWith(
307-
new IntakeRollerAmpIntakeCommand(intakeRollerSubsystem)
308-
)
309-
);
310-
NamedCommands.registerCommand("Feed",
311-
new IntakeRollerFeedCommand(intakeRollerSubsystem).until(intakeRollerSubsystem::getRockwellSensorValue)
312-
);
313-
314258
swerveSubsystem.targetSpeaker();
315259

316260
configureBindings();
@@ -349,169 +293,9 @@ private void configureBindings() {
349293
swerveSubsystem.resetDriverHeading();
350294
}));
351295

352-
/* SWERVE BINDINGS */
353-
354-
/* Shooter Aim -- Holding down the button will change the shooter's pitch to aim it at the speaker. */
355-
// drive
356-
357-
/* Automatic Amping -- Pressing and holding the button will cause the robot to automatically path find to the
358-
* amp and deposit its note. Releasing the button will stop the robot (and the path finding). */
359-
// TODO: Bring back LED integration.
360-
driveController.getAutoAmp().whileTrue(
361-
Commands.deferredProxy(() -> new AutoAmpSequence(
362-
fmsSubsystem,
363-
swerveSubsystem,
364-
elevatorSubsystem,
365-
intakePivotSubsystem,
366-
intakeRollerSubsystem
367-
))
368-
);
369-
370-
// DEPRECATED AMP ALIGN
371-
// driveController.getAmpAlign().onTrue(new InstantCommand(
372-
// () -> lightBarSubsystem.setLightBarStatus(LightBarStatus.AUTO_ALIGN, 1)
373-
// ).andThen(new ParallelRaceGroup(
374-
// AlignCommand.getAmpAlignCommand(swerveSubsystem, fmsSubsystem.isRedAlliance()),
375-
// new ConditionalWaitCommand(
376-
// () -> !driveController.getAmpAlign().getAsBoolean()))
377-
// ).andThen(
378-
// new InstantCommand(() -> lightBarSubsystem.setLightBarStatus(LightBarStatus.DORMANT, 1))
379-
// )
380-
// );
381-
382-
/* Stage Align -- Pressing and holding the button will cause the robot to automatically pathfind such that its
383-
* climb hooks will end up directly above the center of the nearest chain. */
384-
driveController.getStageAlignButton().whileTrue(
385-
Commands.deferredProxy(() -> AlignCommand.getStageAlignCommand(
386-
swerveSubsystem,
387-
swerveSubsystem::getRobotPosition,
388-
fmsSubsystem::isRedAlliance
389-
))
390-
);
391-
392-
393-
/* Note align -- Auto-intakes the nearest visible note, leaving left power control to the driver. */
394-
driveController.getNoteAlign().onTrue(
395-
new AutoIntakeSequence(intakeRollerSubsystem, intakePivotSubsystem,
396-
swerveSubsystem, noteDetector,
397-
driveController, lightBarSubsystem)
398-
.andThen(new IntakePivotSetPositionCommand(intakePivotSubsystem, 0))
399-
.onlyWhile(driveController.getNoteAlign())
400-
);
401-
402296
/* Swerve Stop -- Pressing the button completely stops the robot's motion. */
403297
driveController.getSwerveStop().onTrue(new SwerveStopCommand(swerveSubsystem));
404298

405-
driveController.getShooterAimButton().onTrue(Commands.runOnce(
406-
() -> {
407-
swerveSubsystem.setTargetPoint(fmsSubsystem.isRedAlliance() ? SwerveConstants.RED_SPEAKER_POS : SwerveConstants.BLUE_SPEAKER_POS);
408-
swerveSubsystem.setAim(true);
409-
}
410-
));
411-
412-
driveController.getShooterAimButton().onFalse(Commands.runOnce(
413-
() -> {
414-
swerveSubsystem.setAim(false);
415-
}
416-
));
417-
418-
419-
420-
/* SHOOTER PIVOT TEST */
421-
422-
// rightBumper.onTrue(new ShooterPivotSetAngleCommand(shooterPivotSubsystem,
423-
// Units.degreesToRadians(18)));
424-
425-
// leftBumper.onTrue(new ShooterPivotSetAngleCommand(shooterPivotSubsystem,
426-
// Units.degreesToRadians(60)));
427-
428-
/* SHOOTER PIVOT TUNE */
429-
430-
shooterPivotSubsystem.setDefaultCommand(new InstantCommand(() -> {
431-
//TODO: switch to enum for dpad angles
432-
433-
switch (mechController.getPOV()) {
434-
case 0:
435-
shooterPivotSetPosition += .003;
436-
break;
437-
438-
case 180:
439-
shooterPivotSetPosition -= .003;
440-
break;
441-
442-
case 90:
443-
shooterSpeed += .001;
444-
break;
445-
446-
case 270:
447-
shooterSpeed -= .001;
448-
break;
449-
450-
default:
451-
break;
452-
}
453-
454-
// System.out.println(intakePosition);
455-
456-
457-
458-
// System.out.print(" Speed: " + GRTUtil.twoDecimals(shooterSpeed)
459-
// );
460-
461-
// shooterPivotSubsystem.getAutoAimAngle();
462-
// shooterPivotSubsystem.setAngle(shooterPivotSetPosition);
463-
}, shooterPivotSubsystem));
464-
465-
/* ElEVATOR TEST */
466-
467-
dPadUp.onTrue(new ElevatorToTrapCommand(elevatorSubsystem));
468-
dPadUp.onTrue(new InstantCommand(() -> intakePivotSubsystem.enablePowerLimit(false)));
469-
dPadDown.onTrue(new ElevatorToZeroCommand(elevatorSubsystem));
470-
dPadDown.onTrue(new InstantCommand(() -> intakePivotSubsystem.enablePowerLimit(true)));
471-
472-
473-
dPadLeft.onTrue(new ElevatorToMidCommand(elevatorSubsystem));
474-
dPadLeft.onFalse(new ElevatorToZeroCommand(elevatorSubsystem));
475-
// elevatorSubsystem.setManual();
476-
477-
// elevatorSubsystem.setDefaultCommand(new InstantCommand(() -> {
478-
// if (mechController.getPOV() == 0) {
479-
// elevatorSubsystem.setTargetState(ElevatorState.TRAP);
480-
// // elevatorSubsystem.setMotorPower(0.1);
481-
// } else if (mechController.getPOV() == 180) {
482-
// elevatorSubsystem.setTargetState(ElevatorState.ZERO);
483-
// // elevatorSubsystem.setMotorPower(-0.1);
484-
// }
485-
// else{
486-
// // elevatorSubsystem.setMotorPower(0);
487-
// }
488-
// }, elevatorSubsystem));
489-
490-
elevatorToZero.onTrue(new ElevatorToLimitSwitchCommand(elevatorSubsystem));
491-
/* INTAKE TEST */
492-
493-
// xButton.onTrue(new InstantCommand(() -> intakePivotSubsystem.setPosition(.3),
494-
// intakePivotSubsystem));
495-
496-
// rightBumper.onTrue(new InstantCommand(() ->
497-
// intakePivotSubsystem.setPosition(0), intakePivotSubsystem));
498-
499-
// leftBumper.onTrue(new InstantCommand(() ->
500-
// intakePivotSubsystem.setPosition(.85), intakePivotSubsystem));
501-
502-
/* MECHANISM BINDINGS */
503-
504-
/* Climb Controls -- Left and right joystick up/down controls left and right arm up/down, respectively. Pressing
505-
* down on either the left or right joystick toggles automatic lowering (on by default), which brings down both
506-
* arms automatically when there is no joystick input. */
507-
climbSubsystem.setDefaultCommand(Commands.run(
508-
() -> climbSubsystem.setSpeeds(-mechController.getLeftY(), -mechController.getRightY()), climbSubsystem
509-
));
510-
511-
leftStickButton.onTrue(Commands.runOnce(() -> climbSubsystem.toggleAutomaticLowering(), climbSubsystem));
512-
rightStickButton.onTrue(Commands.runOnce(() -> climbSubsystem.toggleAutomaticLowering(), climbSubsystem));
513-
514-
515299
// rightBumper toggles the amp sequence
516300
// if the elevator is up, lower it and stow the intake
517301
// if the elevator is down, run the amp sequence
@@ -536,50 +320,6 @@ private void configureBindings() {
536320
)
537321
);
538322

539-
// leftBumper toggles the trap position for the elevator
540-
leftBumper.onTrue(
541-
new ConditionalCommand(
542-
new ElevatorToZeroCommand(elevatorSubsystem).alongWith(new InstantCommand(// lower the elevator
543-
() -> {
544-
intakePivotSubsystem.enablePowerLimit(true);
545-
intakePivotSubsystem.setPosition(0);
546-
}, intakePivotSubsystem)), // stow intake
547-
new ConditionalCommand(
548-
new ElevatorToTrapCommand(elevatorSubsystem).andThen(
549-
new InstantCommand(() -> {
550-
intakePivotSubsystem.enablePowerLimit(false);
551-
intakePivotSubsystem.setPosition(.45);
552-
})
553-
),
554-
new IntakePivotSetPositionCommand(intakePivotSubsystem, 1).andThen(// extend pivot
555-
new ConditionalWaitCommand(intakeRollerSubsystem::getFrontSensorReached),
556-
new InstantCommand(() -> intakePivotSubsystem.setPosition(0))
557-
),
558-
intakeRollerSubsystem::getAmpSensor
559-
), // raise the elevator
560-
() -> (elevatorSubsystem.getTargetState() == ElevatorState.AMP // check if targeting a high pos
561-
|| elevatorSubsystem.getTargetState() == ElevatorState.TRAP) && elevatorSubsystem.getExtensionPercent() > .5)
562-
);
563-
564-
565-
// roll behaviors
566-
leftBumper.onTrue(
567-
new ConditionalCommand(
568-
new InstantCommand(),
569-
new ConditionalCommand(
570-
new InstantCommand(),
571-
new WaitCommand(.05).andThen(
572-
new ConditionalWaitCommand(intakePivotSubsystem::atPosition), // extend pivot
573-
new IntakeRollerOuttakeCommand(intakeRollerSubsystem, .17, .75) // run rollers to front sensor
574-
.until(intakeRollerSubsystem::getFrontSensorReached)
575-
),
576-
intakeRollerSubsystem::getAmpSensor
577-
), // raise the elevator
578-
() -> elevatorSubsystem.getTargetState() == ElevatorState.AMP // check if targeting a high pos
579-
|| elevatorSubsystem.getTargetState() == ElevatorState.TRAP)
580-
);
581-
582-
583323

584324
// aButton runs the intake sequence
585325
aButton.onTrue(
@@ -719,34 +459,5 @@ private void configureBindings() {
719459
intakeRollerSubsystem.setRollSpeeds(power, power);
720460
}, intakeRollerSubsystem));
721461

722-
// Offset buttons to correct the shooter if needed
723-
offsetUpButton.onTrue(new InstantCommand(
724-
() -> shooterPivotSubsystem.setAngleOffset(Units.degreesToRadians(3)))
725-
);
726-
offsetUpButton.onFalse(new InstantCommand(
727-
() -> shooterPivotSubsystem.setAngleOffset(Units.degreesToRadians(0)))
728-
);
729-
730-
offsetDownButton.onTrue(new InstantCommand(
731-
() -> shooterPivotSubsystem.setAngleOffset(Units.degreesToRadians(-3)))
732-
);
733-
offsetDownButton.onFalse(new InstantCommand(
734-
() -> shooterPivotSubsystem.setAngleOffset(Units.degreesToRadians(0)))
735-
);
736-
737-
}
738-
739-
/**
740-
* Returns the autonomous command.
741-
*
742-
* @return The selected autonomous command.
743-
*/
744-
public Command getAutonomousCommand() {
745-
return AutoBuilder.buildAuto(autonValue).alongWith(
746-
new ShooterFlywheelReadyCommand(shooterFlywheelSubsystem, lightBarSubsystem),
747-
new InstantCommand(() -> shooterPivotSubsystem.setAutoAimBoolean(true)),
748-
new IntakePivotSetPositionCommand(intakePivotSubsystem, 1),
749-
new ClimbLowerCommand(climbSubsystem)
750-
);
751462
}
752463
}

0 commit comments

Comments
 (0)