Finite state machine problem

[acrilex]

Hello,

I have a problem... One of our team's mentor decided that we would code our robot on a finite state machine format. Our code is available here. However, we can't get any FSM working. Right now, we tested a FSM in BallControl.java. It is initialized in out teleop.java command, and the buttons and motors values are updated in the execute() function. The problem is that, when we enable teleop, it goes straight to state 1 (!). No buttons nor the limitSwitch can change this state. If we disable teleop and re-enable it, it stucks in state 0 (waiting), no buttons can change this state (!). Can someone please help us to correct this problem?

Alexandre Croteau (acrilex)
Head coder,
Team 5179 Sénateurs

[pblankenbaker]

Some General Comments

• Thanks for providing links to your source code - it makes it much easier to help see what might be going on.
• Since you are using a state machine approach, I would strongly encourage you to refactor your robot code to use the SampleRobot. I don't think that the command/subsystem model is not a good fit for your state machine approach.
• If you do use the command/subsystem model, avoid the use of Timer.delay() as the command/subsystem model already has it's own timer delay between every iteration.
• If you do use the command/subsystem model, make sure that you have a mechanism to start your Teleop command (I did not see where you were starting this command).
• I would highly recommend reading the excellent "Command based programming" section (as well as the other sections) that WPIlib provides to Java programmers (http://wpilib.screenstepslive.com/s/4485/m/13809). There are other sections related to choosing your base class.

I am not saying it is impossible to run a state machine using the command/subsystem framework, I'm just guessing it won't be easy.

Here are the major issues that are probably causing the behavior you are seeing:

Teleop

It looks like you intend to use your Teleop command to run your state machine. However, I can't tell where you are starting your Teleop command (what is your trigger to start it?). If you didn't start it anywhere, you might want to create an instance of it in your Robot.robitInit() method and then start it in Robot.teleopInit() (similar to how your autonomous command is started).


BallControl.java

You have declared States as a local variable, set it to 0 and then used a switch statement on the local variable. This results in a "stuck in state 0" condition.

    public static void FSM(){
        // ***** Move declaration of states to class, setting it to 0 here
        // ***** results in your "switch" stuck in case 0.
    	int States = 0;
        // *****
		switch(States){
    	case 0:
    		if (noButtons == true){
    			States = 0;
    			motor = 0;
    			break;
    		}else if(grabButton = true){
    			States = 1;
    			break;
    		}else if(throwButton == true){
    			States = 2;
    			break;
    		}else if(cancelBallButton == true){
    			States = 0;
    			motor = 0;
    			break;
    		}
    	case 1:
    		if (limitSwitch == false){
    			States = 1;
    			motor = -0.75;
    			break;
    		}else if(limitSwitch == true){
    			States = 0;
    			motor = 0;
    			break;
    		}else if(cancelBallButton == true){
    			States = 0;
    			motor = 0;
    			break;
    		}
    	case 2:
    		if (cancelBallButton == false){
    			States = 2;
    			motor = throttle;
    			Timer.delay(2);
    			done = true;
    			break;
    		}else if(done == true){
    			motor = 0;
    			done = false;
    			States = 0;
    			break;
    		}else if(cancelBallButton == true){
    			Timer.getFPGATimestamp();
    			States = 0;
    			motor = 0;
    			break;
    		}
    	}
    }

Teleop.java

Your Teleop command only calls the FSM() method once when it is initialized. That means that your states will never be updated (and your motor output value will never change from 0). Try relocating your FSM() invocation.

    // Called just before this Command runs the first time
    protected void initialize() {
        // ***** Probably can remove from here
    	BallControl.FSM();
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
	Robot.driveTrain.driveWithCorrection(Robot.oi.joystick); // send the joystick input for correction
	BallControl.UpdateButtons();

        // ***** COPY OR MOVE TO HERE
    	BallControl.FSM();

	BallControl.UpdateMotor();

}

[acrilex]

Thank you,

I applied your reccomendations, but I'm worried about one. You said we should move the BallControl.FSM() to Teleop.java's execute function, but wouldn't moving it there make the code stuck on this statement? I think it would execute the first iteration, being stuck at BallControl.FSM() until this command finishes to execute (should never happen if I'm not wrong...), in a kind of infinite loop. I commited your changes and another typo my mentor just pointed out, which was most likely the main problem (in case 0, I typed else if(grabButton = true) insteed of == ). I will get back to you when I test the code during my lunch time.

Alexandre Croteau (acrilex)
Head coder,
Team 5179

[pblankenbaker]

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

[acrilex]

Hello back,

We redesigned our whole code using sample robot. It now works 100%

Thanks for help,

Alexandre Croteau (acrilex)