How we currently do our autonomous is something like this:
Code:
public class Auton {
public int curState = 1;
public Timer timer = new Timer();
public void runAuton() {
switch(curState) {
case 1:
timer.start()
curState = 2
break;
case 2:
//do something
if(timer.get() >= sometime) {
curState = 3;
timer.stop();
timer.reset();
timer.start();
}
break;
case 3:
//So on for how many states you want
break;
}
}
}
Then in your main class you have:
Code:
public class Robot {
public void autonomousPeriodic() {
instanceOfAuton.runAuton();
}
}
This is how we do our state machine, but using an enumeration of AutonState instead of a number.
To do the PD loop you can wire the encoder into a Talon SRX and run it in CAN instead of PWM. From there you can call some methods:
Code:
talon.setFeedbackDevice(FeedbackDevice.QuadEncoder);
talon.changeControlMode(ControlMode.Position) //or speed
talon.setPID(1.0, 0.0 0.01) //PD loop because I is 0
talon.enableControl();
talon.set(somePosition) //Or speed
This way, the talon runs the PID loop internally and you don't have to worry about having a separate encoder object. This is how we are doing the PI loop on our elevator and it is working well so far. Depending on which control mode you are in, the argument passed in the set() method can be a position, speed, voltage, current, or percentage. Percentage is the default -1.0 to 1.0 value.