I do not see any loops in the FSM() method currently (looking at your github repository). So, I don't see how FSM() would ever get stuck in a loop.
Once the Teleop command is started, it's execute() method will be repeatedly invoked by the scheduler until it is interrupted or canceled.
A critical thing to understand is how the "Command Based Programming" model works. It is designed to allow multiple commands run simultaneously, but requires cooperation between the commands.
Things like the Timer.delay(2) in your FSM() method will typically cause all sorts of problems when using the "Command Based Programming" model. Imagine the following:
- You have one command that is scheduled to run that reads joystick values and uses those values to adjust the power to the drive train.
- Your FSM() method in the BallControl subsystem is invoked such that it falls through the case 2 condition where you do a Timer.delay(2).
- During that 2 second delay, all other commands will stop updating.
- This means that if your robot was going full speed forward, it would continue to do so for 2 seconds when this condition was reached (the driver would have no control during this 2 second sleep).
I would recommend that you "start over" and not try to combine a state machine with the command based framework (or keep track of states in your commands and not in your subsystems).
Our team has been using the Command/Subsystem framework for a few years now and we really like it. There are always going to be special cases where it can be frustrating, but overall it is well thought out and helps to avoid the complexity of large state machines.
Our general rule of thumb for Java programmers getting started with this framework is as follows:
- Force yourself to read all the way through the Command based programming documents at least one (I gave the link in my last post). It is critical to understand the design of this framework before attempting to use it.
- Keep your sub-systems stupid (provide simple methods to read inputs and read/set output).
- Put the smarts into each Command you implement.
- Start with the creation of basic commands (DriveWithJoysticks, DriveStraight, PrepareLaunch, LaunchBoulder) and learn how to chain them together.
- Test your commands using the SmartDashboard. Like: SmartDashboard.putData("Test Drive Straight", new DriveStraight(0.5, 2.0)).
- Use SmartDashboard.putData("BallControl", Robot.ballControl) to see what command is currently running on a subsystem (you only need to do this once during initialization)
- Use the JoystickButton class to fire off commands when a button is pressed, released, toggled or held.
Good Luck