Autonomous in iterative robot

[Tparbotmail]

We are very uncertain on how to run autonomous in iterative robot. I have searched much code on github and read tons of post here. I don't see a lot of Boolean management in the code I have have found. Using Boolean logic to protect sections of logic was the only way we could get autonomous to work at all last year. What that means is, that the logic in the first part of the autonomous is executed based on some Boolean condition, while later logic is protected by other Boolean logic whose conditions have not been met. This is just an example, I am not talking about timers, it applies to any logic in iterative autonomous.

If timerA < 5 && a==true && b==false && c==false
Then do some work the next section of logic is dependent on
Since timeA is done set it's Boolean a=false and b=true
If timerB < 2 && a==false && b==true && c==false
Then do some more work and complete by managing more Booleans

Unless we used Booleans to mange state at any given time the robot would not work because all the code would execute simultaneously. We can handle Boolean arrays and the for loops to load them. We are really struggling to get any autonomous working at all because of the amount of Booleans we need to protect all the logic from executing simultaneously.

So here is the question I am trying to answer. I see posts here and code in github but see very little if anything about autonomous iterative and Boolean management. If anything I see the opposite. I see these very elegant code sections but as far as i can tell they won't work. Are we missing something about autonomous iterative that everyone is doing to keep the logic from executing simultaneously, but we have missed? Any guidance would be greatly appreciated.

Thanks

[Oromus]

One of the simplest and easiest ways to have logic for autonomous not run at the same time is to use Thread.sleep(). It lets you run code, have the code halt for a set amount of milliseconds/seconds, then proceed with more code. An example:

public void autonomousInit() {
    try {
        leftMotor.set(1);
        rightMotor.set(1);
        Thread.sleep(1000); //Thread.sleep() takes milliseconds, so 1000 milliseconds = 1 second
        leftMotor.set(0);
        rightMotor.set(0);
    } catch (InterruptedException e) { //Thread.sleep() can throw an InterruptedException, so we have to catch it
        e.printStackTrace();
    }
}

This code makes the robot drive forward for one second and then stop. The only potential issues with this is that Thread.sleep() halts all code execution on that Thread, so if there is other code that needs to be called constantly you'll either need to not use Thread.sleep() or do multi-threading. In a simple autonomous like this, though, Thread.sleep() should fit your needs perfectly. Just remember that no code after Thread.sleep() will be called until after the delay. Hope this helps!

Also, in the event you want to do multi-threading, let me know and I'll help you out there

[pblankenbaker]

Sorry, for the earlier post (I posted my answer to a different thread to this thread by mistake).

Here are some suggestions to avoid the long boolean array pattern:

• Try transitioning between "states" to keep things more manageable in your code.
• I would also recommend that your team spend some time evaluating the Command based framework after this season ends to see if it might be a better fit for next year (it addresses this type of issue with an entirely different pattern). Or, if you have a lot of time waiting for a turn on the robot, you might want to investigate it now in a new project to see if it would be a better fit for what you want to accomplish.

As far as using a state based iterative method, there are probably many approaches. Here is a rough framework that include a timer to get you started:

    // Keeps track of time state was entered
    private Timer autonStateTimer;

    // Keeps track of current state
    private int autonState;

    // List of possible states    
    private final static int AUTON_STATE_DRIVE_FORWARD = 1;
    private final static int AUTON_STATE_STOP = 2;
    private final static int AUTON_STATE_SHOOT = 3;
    private final static int AUTON_STATE_FINISHED = 4;
    
    // Helper method to change to new state and reset timer so
    // states can keep track of how long they have been running.

    private void changeAutonState(int nextState) {
    	if (nextState != autonState) {
    		autonState = nextState;
    		autonStateTimer.reset();
    	}
    }
    
    public void autonomousInit() {
    	// Reset auton state to initial drive forward and reset the timer
    	autonState = AUTON_STATE_DRIVE_FORWARD;
    	autonStateTimer = new Timer();
        // Not sure if start() is required anymore, but it shouldn't hurt
        autonStateTimer.start();
    }

    public void autonomousPeriodic() {
    	switch (autonState) {
    	
    	case AUTON_STATE_DRIVE_FORWARD: {
    		// Drive forward at half power for 3 seconds
    		setDrivePower(0.5, 0.5);
    		if (autonStateTimer.hasPeriodPassed(3.0)) {
    			changeAutonState(AUTON_STATE_STOP);
    		}
    		break;
    	}

    	case AUTON_STATE_STOP: {
    		// Turn off drive motors
    		setDrivePower(0.0, 0.0);
    		// After 1/2 elapses (time to stop) transition to
    		// shooter state
    		if (autonStateTimer.hasPeriodPassed(.5)) {
    			changeAutonState(AUTON_STATE_SHOOT);
    		}
    		break;
    	}

    	case AUTON_STATE_SHOOT: {
    		// Some auton method that fires a boulder
    		fireBoulder();
    		if (autonStateTimer.hasPeriodPassed(1.5)) {
    			changeAutonState(AUTON_STATE_FINISHED);
    		}
    		break;
    	}

    	case AUTON_STATE_FINISHED: {
    		stopShooter();
    		break;
    	}
    	}

    }

The switch/case code can get pretty long when using this pattern. It can be easier to manage (read) if you have each case call a separate method. You will need to implement the methods you want to perform in each state - the calls above are just pseudo code.

Hope that helps.

[mmaunu]

Originally Posted by Oromus

One of the simplest and easiest ways to have logic for autonomous not run at the same time is to use Thread.sleep(). It lets you run code, have the code halt for a set amount of milliseconds/seconds, then proceed with more code. An example:

public void autonomousInit() {
    try {
        leftMotor.set(1);
        rightMotor.set(1);
        Thread.sleep(1000); //Thread.sleep() takes milliseconds, so 1000 milliseconds = 1 second
        leftMotor.set(0);
        rightMotor.set(0);
    } catch (InterruptedException e) { //Thread.sleep() can throw an InterruptedException, so we have to catch it
        e.printStackTrace();
    }
}

This code makes the robot drive forward for one second and then stop. The only potential issues with this is that Thread.sleep() halts all code execution on that Thread, so if there is other code that needs to be called constantly you'll either need to not use Thread.sleep() or do multi-threading. In a simple autonomous like this, though, Thread.sleep() should fit your needs perfectly. Just remember that no code after Thread.sleep() will be called until after the delay. Hope this helps!

Also, in the event you want to do multi-threading, let me know and I'll help you out there

Be careful with this. If you are doing this (putting large delays) inside of an IterativeRobot or Command-based project, you will not get good results as you are delaying the control loops, which are called every 20ms. You also wouldn't want to put it in autonomousInit(); use autonomousPeriodic() to control your autonomous routine instead. This style, using a sleep or delay command, works in SampleRobot projects.

OP, I would recommend going with the state machine approach that pblankenbaker describes. It closely mirrors what you are currently doing but adds more context and manageability.

[Tparbotmail]

These are all good suggestions and thank you. I am very interested in multi threading not quite sure how it works, but am pretty sure We are going to need it based on my understanding. I'd love to hear about it and see examples.

My question was more of a sanity check because I don't see a lot of Boolean and state discussion in Delphi or in code on git hub. I don't see the Boolean management technique that we used last year with our autonomous routine. So I am wondering if I have missed something obvious, like checking to see if the computer is plugged in, or the wpilib page in the Java section that demonstrates 10 ways to run iterative autonomous. On my team we call it "iterative choking" when the wheels start sputtering we know that that iterative is executing two different motor speeds simultaneously because our logic is flawed and lacks Boolean gatekeepers.

Is the problem really about managing state in autonomous? Maybe I am searching on the wrong terms. The example with the case statements is really good and make sense. We are actually trying to avoid timers this year and rely on sensors. I use timers in my example because timers are used a lot by everyone. Do you guys experience iterative choking too? Or did I completely miss the lecture on iterative autonomous?

Thanks

[mikets]

Originally Posted by Tparbotmail

These are all good suggestions and thank you. I am very interested in multi threading not quite sure how it works, but am pretty sure We are going to need it based on my understanding. I'd love to hear about it and see examples.

My question was more of a sanity check because I don't see a lot of Boolean and state discussion in Delphi or in code on git hub. I don't see the Boolean management technique that we used last year with our autonomous routine. So I am wondering if I have missed something obvious, like checking to see if the computer is plugged in, or the wpilib page in the Java section that demonstrates 10 ways to run iterative autonomous. On my team we call it "iterative choking" when the wheels start sputtering we know that that iterative is executing two different motor speeds simultaneously because our logic is flawed and lacks Boolean gatekeepers.

Is the problem really about managing state in autonomous? Maybe I am searching on the wrong terms. The example with the case statements is really good and make sense. We are actually trying to avoid timers this year and rely on sensors. I use timers in my example because timers are used a lot by everyone. Do you guys experience iterative choking too? Or did I completely miss the lecture on iterative autonomous?

Thanks

A lot of autonomous strategies are sequential, meaning you start an operation, you wait for it to complete before starting the next operation. That's what the switch/case state machine will do for you. You start an operation in one state, move to the next state to wait for it to complete before moving onto another state. However, it may be beneficial to start multiple operations simultaneously in the same state if these operations do not have dependencies on each other. For example, you may want to raise your elevator at the same time as driving the robot forward. There is no reason why you can't do that at the same time. But you don't need multi-threading to do this. You just have to check for multiple conditions before moving onto another state. For example, hypothetically, let's say you have some sort of elevator that will reach up to the high goal to score the boulder. You can do this sequentially, drive the robot forward to the castle wall then raise the elevator. But then it is wasting precious time. You can start driving forward as well as raising the elevator at the same time but you can't score the boulder until both operations are completed. You can still achieve this with a simple state machine:

switch (state)
{
    case 1:
        //
        // Keep driving forward and raising elevator until targets are reached.
        // When both targets are reached, move to the next state.
        //
        boolean reachedWall = sonarDistance <= stopDistance;
        boolean reachedHighGoal = elevatorHeight >= goalHeight;

        if (reachedWall)
            tankDrive(0.0, 0.0);
        else
            tankDrive(0.5, 0.5);
        
        if (reachedHighGoal)
            elevatorMotor.set(0.0);
        else
            elevatorMotor.set(0.5);

        if (reachedWall && reachedHighGoal)
            state++;

        break;

    case 2:
        scoreMechanism.extend();
        break;

    ...

In my opinion, multi-threading is not needed in most scenarios. It gives you more trouble than it's worth unless you understand issues with multi-threading programming (e.g. resource contention and thread synchronization). Of all the years doing robot programming, we developed a multi-tasking robot framework library that uses only a single thread. There is only one exception: vision targeting. And that's only because the NI vision processing code is a blocking call. Calling it will affect our robot's responsiveness. We took care of it in our library by spawning a separate thread doing vision processing. That's the only exception.

[geniusadam69]

Are using a command based approach? The command based approach seems to make it pretty simple. Keep grouping commands together until you get up to the full autonomous command.

If you're trying to maintain something in a state based approach, i would instead use an enum to manage the state. Then it could only be in one state and those states will have commands or actions assigned to them. It will also make it easier to write the logic to determine your state.

Let me know if you have more questions.

[Tparbotmail]

Originally Posted by mikets

A lot of autonomous strategies are sequential, meaning you start an operation, you wait for it to complete before starting the next operation. That's what the switch/case state machine will do for you. You start an operation in one state, move to the next state to wait for it to complete before moving onto another state. However, it may be beneficial to start multiple operations simultaneously in the same state if these operations do not have dependencies on each other. For example, you may want to raise your elevator at the same time as driving the robot forward. There is no reason why you can't do that at the same time. But you don't need multi-threading to do this. You just have to check for multiple conditions before moving onto another state. For example, hypothetically, let's say you have some sort of elevator that will reach up to the high goal to score the boulder. You can do this sequentially, drive the robot forward to the castle wall then raise the elevator. But then it is wasting precious time. You can start driving forward as well as raising the elevator at the same time but you can't score the boulder until both operations are completed. You can still achieve this with a simple state machine:

switch (state)
{
    case 1:
        //
        // Keep driving forward and raising elevator until targets are reached.
        // When both targets are reached, move to the next state.
        //
        boolean reachedWall = sonarDistance <= stopDistance;
        boolean reachedHighGoal = elevatorHeight >= goalHeight;

        if (reachedWall)
            tankDrive(0.0, 0.0);
        else
            tankDrive(0.5, 0.5);
        
        if (reachedHighGoal)
            elevatorMotor.set(0.0);
        else
            elevatorMotor.set(0.5);

        if (reachedWall && reachedHighGoal)
            state++;

        break;

    case 2:
        scoreMechanism.extend();
        break;

    ...

In my opinion, multi-threading is not needed in most scenarios. It gives you more trouble than it's worth unless you understand issues with multi-threading programming (e.g. resource contention and thread synchronization). Of all the years doing robot programming, we developed a multi-tasking robot framework library that uses only a single thread. There is only one exception: vision
targeting. And that's only because the NI vision processing code is a blocking call. Calling it will affect our robot's responsiveness. We took care of it in our library by spawning a separate thread doing vision processing. That's the only exception.

I did do quite a bit of coding tonight using these techniques and I am seeing case 2 logic being executed when case1 is the state iterative autonomous. If the encoder values I am checking for are true in both case1 and case2 then the sequence becomes unstable. The only way I can think to protect case1 logic from case2 is to use Booleans
Case1:
If (encoder < 400 && case1-Boolean == true && case2-Boolean == false)

Then do some work

State++
Break;
Case2:
If (encoder < 400 && case1-Boolean == false && case2-Boolean == true)

Do some other work.

Is there a way in eclipse to step throu the code while it is executing on the rio?

[mikets]

Originally Posted by Tparbotmail

I did do quite a bit of coding tonight using these techniques and I am seeing case 2 logic being executed when case1 is the state iterative autonomous. If the encoder values I am checking for are true in both case1 and case2 then the sequence becomes unstable. The only way I can think to protect case1 logic from case2 is to use Booleans
Case1:
If (encoder < 400 && case1-Boolean == true && case2-Boolean == false)

Then do some work

State++
Break;
Case2:
If (encoder < 400 && case1-Boolean == false && case2-Boolean == true)

Do some other work.

Is there a way in eclipse to step throu the code while it is executing on the rio?

Sorry, I am not sure I understand what you are describing. Are you saying while state is 1, the logic in state 2 is executed? That's impossible unless you have a code path that did not have a "break" and the code fell through to the next case.

[pblankenbaker]

You can step through a Robot program in Eclipse using the debug mode and break points. See:

http://wpilib.screenstepslive.com/s/...-robot-program

Be very, very careful when debugging code that involves movement. You may need to unplug motors or at a minimum make sure your robot is up on blocks.

[Tparbotmail]

Thank you both for the reply. It does have breaks, and I instrumented it so I could see where the code was in the console. It clearly was jumping into the 2nd if statement in the second case statement because it was checking for the same encoder value range.

We saw the same behavior last year and it was resolved with an army of Booleans. Either I missed the lecture on autonomous, or console cannot keep up, or what I a trying to do is more ambitious than the rio can handle. i am to see if there is a way to tail output from a log in putty.

[mmaunu]

Originally Posted by Tparbotmail

I did do quite a bit of coding tonight using these techniques and I am seeing case 2 logic being executed when case1 is the state iterative autonomous. If the encoder values I am checking for are true in both case1 and case2 then the sequence becomes unstable. The only way I can think to protect case1 logic from case2 is to use Booleans
Case1:
If (encoder < 400 && case1-Boolean == true && case2-Boolean == false)

Then do some work

State++
Break;
Case2:
If (encoder < 400 && case1-Boolean == false && case2-Boolean == true)

Do some other work.

Is there a way in eclipse to step throu the code while it is executing on the rio?

Try putting an else on your second condition? Is it possible that the first condition was true and then the second condition was also true...without an "else if", both can run.

[mikets]

Originally Posted by Tparbotmail

Thank you both for the reply. It does have breaks, and I instrumented it so I could see where the code was in the console. It clearly was jumping into the 2nd if statement in the second case statement because it was checking for the same encoder value range.

We saw the same behavior last year and it was resolved with an army of Booleans. Either I missed the lecture on autonomous, or console cannot keep up, or what I a trying to do is more ambitious than the rio can handle. i am to see if there is a way to tail output from a log in putty.

Would you post the actual code? Like I said it is impossible to execute both case 1 and 2 unless you missed a break in one of the code paths.

[ahartnet]

It's late in the game for a change, but depending on how much code you've written so far, definitely look into the command based programming (can be found here: http://wpilib.screenstepslive.com/s/.../13809/c/88893 (or the full pdf manual here: http://wpilib.screenstepslive.com/s/4485/m/13809/pdf)

I've done state based programming before, and was kind of my "go to" way of programming. Command Based has a lot of advantages over the state machine way, and doesn't have that big of a learning curve if you already feel comfortable programming in Java

[Tparbotmail]

Hi mikets
Thanks for replying. I am hesitant on putting the code in because they often turn into code critiques rather than try to fix the problem. I'd send it to you as a private message after I clean it up.

We tried lots of different things, tonight including calling classes to make the motors go. The robot is on blocks. Essentially the first case statement reads the encoder to 10K clicks, then switches, then reads between 10K and 20K clicks, and then switches into the final logic which changes state to go to the next case. It resets the encoder to zero, increments the state variable pop's into the control logic in case two and sputters like it's executing 3 different motor speeds simultaneously. What I call iterative choking.

Maybe it is a CANTalon thing. The bigger problem is this was supposed to be a simple proof of concept to have the robot drive in a straight line and stop. So we could measure the actual distance to speed calculations. It's really turned into a major challenge. I am not ruling out operator error at all, however, I am not so certain these statements are really protecting the motors from other code. We checked for break. We will keep checking for logic errors. I will send you the code when I clean it up in a private email.