[F22Rapture]

Are you using the Command-Based template? Create a command for each thing you want to do, then string them together with a Command Group.

In RobotMain

private Command autonomousCommand;

public void autonomousInit() {
        autonomousCommand = new Auton0();    
        autonomousCommand.start();
    }

public void autonomousPeriodic() {
        Scheduler.getInstance().run();
    }

In command group

public class Auton0 extends CommandGroup {
    
    public Auton0() {
        addSequential(new SetFrontSpeed(50);
        addParallel(new SetBackSpeed(50);
        addSequential(new SetAngle(90));
        addSequential(new WaitCommand(3));
        addSequential(new SetAngle(45));
    
        etc. etc.
    }