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;
private Hood hood = null;
private Spindexer spindexer = null;
private Intake intake = null;
+ private LED led = null;
// this is inside addAuto()
// private Command auto = new DoNothing();
// Controllers are defined here
private BaseDriverConfig driver = null;
private Operator operator = null;
- private LED led = 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<>();
// Spindexer
public static final int SPINDEXER_ID = 4;
- // Climb
- public static final int CLIMB_MOTOR_ID = 8;
-
// Intake
public static final int RIGHT_MOTOR_ID = 1;
public static final int LEFT_MOTOR_ID = 2;
private Spindexer spindexer;
// PS5 button aliases
- private final Button CROSS = Button.A;
+ // private final Button CROSS = Button.A;
private final Button CIRCLE = Button.B;
private final Button SQUARE = Button.X;
- private final Button TRIANGLE = Button.Y;
- private final Button LB = Button.LB;
+ // private final Button TRIANGLE = Button.Y;
+ // private final Button LB = Button.LB;
private final Button RB = Button.RB;
private final Button CREATE = Button.BACK;
- private final Button OPTIONS = Button.START;
+ // private final Button OPTIONS = Button.START;
private final Button LEFT_JOY = Button.LEFT_JOY;
private final Button RIGHT_JOY = Button.RIGHT_JOY;
private final Axis LEFT_Y = Axis.LEFT_Y;
private final Axis RIGHT_X = Axis.RIGHT_X;
private final Axis RIGHT_Y = Axis.RIGHT_Y;
- private final Axis LEFT_TRIGGER = Axis.LEFT_TRIGGER;
- private final Axis RIGHT_TRIGGER = Axis.RIGHT_TRIGGER;
+ // private final Axis LEFT_TRIGGER = Axis.LEFT_TRIGGER;
+ // private final Axis RIGHT_TRIGGER = Axis.RIGHT_TRIGGER;
public PS5XboxModeDriverConfig(
Drivetrain drive,
if(calibrating){
leftMotor.set(-0.1);
rightMotor.set(-0.1);
- boolean atHardStop = Math.abs((leftMotor.getStatorCurrent().getValueAsDouble() + rightMotor.getStatorCurrent().getValueAsDouble()) / 2) >= IntakeConstants.CALIBRATING_CURRENT_THRESHOLD;
}
updateInputs();
public int ballCount = 0;
private boolean wasSpindexerSlow = false;
private SpindexerState state = SpindexerState.STOPPED;
- private boolean reversing = false;
private SpindexerIOInputsAutoLogged inputs = new SpindexerIOInputsAutoLogged();
public boolean noIndexing = false;
if (state == SpindexerState.MAX) {
motor.setControl(new DutyCycleOut(SpindexerConstants.spindexerMaxPower).withEnableFOC(true));
- reversing = false;
} else if (state == SpindexerState.REVERSE) {
motor.setControl(new DutyCycleOut(SpindexerConstants.spindexerReversePower).withEnableFOC(true));
- reversing = true;
} else if (state == SpindexerState.STOPPED) {
motor.setControl(new DutyCycleOut(0.0).withEnableFOC(true));
- reversing = false;
} else if (state == SpindexerState.RESET && resetPos != null) {
motor.setControl(new DutyCycleOut(resetPID.calculate((motor.getPosition().getValueAsDouble() / gearRatio) % 1.0, resetPos)).withEnableFOC(true));
} else {
motor.setControl(new DutyCycleOut(power).withEnableFOC(true));
- reversing = false;
}
public static void registerPlaceholderCommands() {
NamedCommands.registerCommand("Extend intake", new InstantCommand());
NamedCommands.registerCommand("Intake", new InstantCommand());
- NamedCommands.registerCommand("Climb", new InstantCommand());
}
/**