OK-- so my first reply talks to how to sequence commands.
The second reply talks about how to set it up to strafe as the original post also asked.
But I just reread the most recent post and this is asking how to sequence after a pid movement.
In your pidClass you can add a method called onTarget() which returns true when the PID has reached its target.
Code:
class PDLoop {
...
public boolean isOnTarget() {
if(Math.abs(source.pidGet() - m_controller.getSetpoint()) < 1) {
//within one inch, be done
return true;
}
}
You could also disable when you return true too.
Also you may wish to see isOnTarget()==true for say 5 periods or longer to be sure you don't sample as it is overshooting.
Or you could wait until you are both onTarget and the encoder's rate is 0;
Above I provided a way to sequence commands through a simple state counter. I used time for the classe DriveForTime(). DriveForDistance might look something like
Code:
class xxxx extends IterativeRobot {
...
private class DriveForDistance {
boolean m_firstTimeThrougRun=true;
double m_distance;
public DriveForDistance (double distance)
{
m_distance=distance;
}
public void runPeriodic()
{
if(m_firstTimeThroughRun) {
PDLoop.driveForDistance(m_distance);
}
if(PDLoop.isOnTarget() && Math.abs(drivetrain.encoder.getRate()) < 0.1) {
m_state++;
}
}
}
DriveForDistance driveNinetyInches;
public void autonomousInit() {
driveNinetyInches=new DriveForDistance(90);
...
}
autonomousPeriodic()
{
//sequencing to drive forward at half power for 2s, turn left for one second and then backup at 80% power for 5s, and lift the forks at half power for 2s.
if(m_state==0) {
driveNinetyInches.runPeriodic();
} else if (m_state==1) {
turnLeftOneSecond.runPeriodic();
} else if (m_state==2) {
backupFiveSeconds.runPeriodic();
} else if (m_state==3) {
liftTwoSeconds.runPeriodic();
}
}