[cstelter]

Originally Posted by 2386programming

We are using an iterative robot.

So, with IterativeRobot you are putting your code in

autonomsousInit()
{
   //stuff to set intial state
}

autonomousPeriodic()
{
    //sequencing
}

I'm probably not the best one to be giving advice as I've never helped a team program in Iterative Robot directly-- only command based.

But my advice would be to set up some sort of rudimentary state machine to keep things sane. You could break each step into a separate function (or better yet as I show below, a different object from a class with a function that makes it run) and then call the correct function based on the phase you are in.

Note, I just typed this code into the browser-- it no doubt has mistakes and is only given as a rough guideline. Hopefully the next 12 posts won't be folks pointing out errors, but that could be entirely possible. It's not intended to be dropped in and run, but just an example to organize a raw Iterative Robot without commands and the scheduler.

class xxxx extends IterativeRobot {
   int m_state;

   DriveForTime driveTwoSeconds;
   DriveForTime turnLeftOneSecond;
   DriveForTime backupFiveSeconds;
   MoveForksForTime liftTwoSeconds;

autonomsousInit()
{
   //stuff to set intial state
   DriveForTime driveTwoSeconds=new DriveForTime(2.0,0.5,0.5);
   DriveForTime turnLeftOneSecond=new DriveForTime(1.0,-0.5,0.5);
   DriveForTime backupFiveSeconds= new DriveForTIme(5.0,-0.8,-0.8);
   MoveForksForTime liftTwoSeconds= new MoveForksForTime(2.0,0.5);
   m_state=0;
}

class DriveForTime {
   Timer m_timer;
   boolean m_firstTimeThroughRun=true;
   double m_time;
   double m_leftSpeed;
   double m_rightSpeed;

   //constructor-- called when you call new on DriveForTime
   DriveForTime(double time, double leftSpeed, double rightSpeed) {
       m_time=time;
       m_leftSeed=leftSpeed;
       m_rightSpeed=rightSpeed;
   }

   //This will get called once each time thorugh autonomousPeriodic
   public void runPeriodic() {
       driveTrain.tankDrive(m_leftSpeed,m_rightSpeed);
       if(m_firstTimeThroughRun) {
          //start the timer and reset m_firstTimeThroughRun flag
          m_timer.reset();
          m_timer.start();
          m_firstTimeThroughRun=false;
       }
 
       if(m_timer.hasPeriodPassed(m_time)) {
            //we're done, advance state counter
            driveTrain.tankDrive(0,0);
            m_state++;
       }
}

class MoveForksForTime {
   Timer m_timer;   
   boolean m_firstTimeThroughRun=true;   //Allow starting timer first time run() is called
   double m_time;    //how long to run forks
   double m_speed;   //power to run forks

   //constructor-- this will be called when you new a MoveForksForTime
   MoveForksForTime(double time, double speed) {
       m_time=time;
       m_speed=speed;
   }
   public void runPeriodic() {
       forksMotor.set(speed);

       if(m_firstTimeThroughRun) {
          //start the timer and reset m_firstTimeThroughRun flag
          m_timer.reset();
          m_timer.start();
          m_firstTimeThroughRun=false;
       }
 
       if(m_timer.hasPeriodPassed(m_time)) {
            //we're done, advance state
            forksMotor.set(0);
            m_state++;
       }
}



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) {
        driveTwoSeconds.runPeriodic();
   } else if (m_state==1) {
        turnLeftOneSecond.runPeriodic();
   } else if (m_state==2) {
        backupFiveSeconds.runPeriodic();
   } else if (m_state==3) {
        liftTwoSeconds.runPeriodic();
   }
}

By using object oriented techniques, one can hide all the timer code that exists and is duplicated in both MoveForksForTime and DriveForTime in a base class requiring you to only write the motor driving mentod.

But this is just a *very* cheap/crude version of what the Scheduler class and the Command class can do for you. Going down the path above is simply reinventing a very substantial wheel that already exists for us. But if you just can't make that jump, and you can understand the above, at least it helps lend order to what otherwise can end up being a nightmare of nested if/then/else code.

I would strongly recommend you consider going to Commands and Subsystems. The above approach can get you by in a pinch and might make more intuitive sense because you're writing all the code to sequence your commands by maintaining the m_state variable.

The Scheduler lets you create unique commands like I did simply above that require certain parts of your robot called subsystems (maybe the drivetrain or a motor and an encoder). The command has predefined phases that the scheduler calls (initialize(), execute(), end(), isFinished(), etc.), and once you fill out those items, you just have to start your command in the scheduler and will take over exclusive rights to the subsystem and operate until it declares it is finished or until another command also needing the subsystem is scheduled.

The Scheduler keeps track of all scheduled commands and keeps them firing each time through teleopPeriodic() or autonomousPeriodic()-- both of those functions become a one line Scheduler.getInstance().run() call.