import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.wpilibj.DriverStation;
- import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.PS5Controller;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+ import frc.robot.commands.DoNothing;
import frc.robot.commands.LogCommand;
import frc.robot.commands.drive_comm.DefaultDriveCommand;
- import frc.robot.commands.gpm.AutoShootCommand;
- import frc.robot.commands.gpm.ClimbDriveCommand;
-import frc.robot.commands.gpm.BrownOutControl;
import frc.robot.commands.gpm.IntakeMovementCommand;
import frc.robot.commands.gpm.LockedShoot;
import frc.robot.commands.gpm.RunSpindexer;
import frc.robot.controls.BaseDriverConfig;
import frc.robot.controls.Operator;
import frc.robot.controls.PS5ControllerDriverConfig;
- import frc.robot.subsystems.Climb.LinearClimb;
import frc.robot.subsystems.Intake.Intake;
import frc.robot.subsystems.LED.LED;
+import frc.robot.subsystems.PowerControl.EMABreaker;
import frc.robot.subsystems.drivetrain.Drivetrain;
import frc.robot.subsystems.drivetrain.GyroIOPigeon2;
import frc.robot.subsystems.hood.Hood;
// Controllers are defined here
private BaseDriverConfig driver = null;
private Operator operator = null;
- private LinearClimb linearClimb = null;
- private LED led = null;
+ private EMABreaker breaker = null;
+
+ // TODO: move to correct robot and put the correct port?
+ private PS5Controller ps5 = new PS5Controller(0);
+
// auto Command selection
private final SendableChooser<Command> autoChooser = new SendableChooser<>();
if (turret != null) {
turret.setDefaultCommand(new Superstructure(turret, drive, hood, shooter, spindexer));
}
-
- if (shooter != null && spindexer != null && turret != null && intake != null && hood != null && drive != null) {
- CommandScheduler.getInstance().schedule(new BrownOutControl(shooter, spindexer, turret, intake, hood, drive));
- }
- drive.setDefaultCommand(new DefaultDriveCommand(drive, driver));
+ if (drive != null && driver != null) {
+ drive.setDefaultCommand(new DefaultDriveCommand(drive, driver));
+ }
break;
}