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.
Code:
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.
Code:
// 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();
}