public TurnToHeadingCommand(
RobotSubsystems robotSubsystems,
RobotControllers robotControllers,
double degrees,
boolean isRelative) {
this.powertrainSubsystem = robotSubsystems.getPowertrainSubsystem();
this.currentHeading = powertrainSubsystem.getGyroAngle();
this.isRelative = isRelative;
this.pidController = robotControllers.getGyroTurnPIDController();
if (this.isRelative) {
this.setpoint = currentHeading + degrees;
} else {
this.setpoint = degrees;
}
requires(powertrainSubsystem);
}
public TwoBallStaticShootSlightRightAutonomous(
RobotSubsystems robotSubsystems,
RobotControllers robotControllers,
VisionProcessing visionProcessing) {
double liftIntakeSpeed =
robotSubsystems.getRobotConfig().getIntakeConfig().getLiftIntakeSpeed();
addSequential(
new CrossStaticDefenseAndShootForwardAutonomous(
robotSubsystems, robotControllers, visionProcessing));
addSequential(new TurnToHeadingCommand(robotSubsystems, robotControllers, 0, false));
// TODO Add command to turn to ball
addParallel(new RunRollerbarIntakeAtSpeedCommand(robotSubsystems, -liftIntakeSpeed), 1.75);
addSequential(new DriveInchesCommand(robotSubsystems, robotControllers, -18));
addSequential(new EmptyCommand(), .3);
addSequential(new RunIntakeLiftAtSpeedCommand(robotSubsystems, liftIntakeSpeed), .25);
addSequential(new DriveInchesCommand(robotSubsystems, robotControllers, 18));
addSequential(new TurnToHeadingCommand(robotSubsystems, robotControllers, 0, false));
addSequential(
new CrossStaticDefenseAndShootForwardAutonomous(
robotSubsystems, robotControllers, visionProcessing));
}