-
Notifications
You must be signed in to change notification settings - Fork 4
[Tutorial 1.6] Basic Autonomous
See the readme on the main page for the correct documentation.
[Back to the previous tutorial]([Tutorial 1.5] Organizing your code)
In FRC competitions, usually the first ten or fifteen seconds of each match are spent in Autonomous mode, where the drivers don't get to touch the controls and the robot has to move autonomously.
There are multiple ways to structure such code, but often the most understandable is as a sequence of operations expressed as normal Java code. This, however, isn't often the way that autonomous modes are written in FRC programming, but the CCRE makes this very easy to do.
Let's add the structure of Autonomous to our file:
Igneous.registerAutonomous(new InstinctModule() {
protected void autonomousMain() throws AutonomousModeOverException, InterruptedException {
// Autonomous code goes here.
}
});
This will create a new InstinctModule, which represents a single autonomous mode controller. By registering it with Igneous.registerAutonomous, it will automatically start and stop along with autonomous mode.
Let's make the robot drive forward in Autonomous mode. Put this code instead of // Autonomous code goes here.:
leftDriveMotor.set(1);
rightDriveMotor.set(1);
Run it in the emulator, switch to Autonomous mode, and hit enable! Notice how the motors move to driving at full forward.
Now disable the robot, and notice how the motors remain the same. On a real robot, the motors would be stopped because they cannot move while the robot is disabled.
Now enable the robot in Teleoperated mode. Notice how they don't instantly snap to the new position - this is because the ramping takes time to change them. However, this gives us a problem - when we first enable it in Teleoperated, the robot will jerk for a moment as the ramping returns to zero.
Let's fix this. Add these lines outside the new mode:
FloatMixing.setWhen(Igneous.startDisabled, leftDriveMotor, 0);
FloatMixing.setWhen(Igneous.startDisabled, rightDriveMotor, 0);
Now try it again in the emulator. Now when you disable the robot, the motors return to zero! That's better.
Our autonomous mode is currently very boring. Let's do something more interesting!
InstinctModules can wait for various things to occur before continuing on. The simplest one of these is waiting for a specified amount of time. Let's add three lines to our autonomous mode to make it look like this:
leftDriveMotor.set(1);
rightDriveMotor.set(1);
waitForTime(1000);
leftDriveMotor.set(0);
rightDriveMotor.set(0);
'waitForTime(milliseconds)` will wait for that amount of time, and then continue. If the autonomous mode ends while this is running, it will throw AutonomousModeOverException and/or InterruptedException, and this will cause the autonomous mode to end. Don't ever catch these exceptions!
Let's try this code out on the emulator. Notice how the motors go up and then back down after a second.
You may want to experiment with adding more waits and more complex behaviors here before continuing.
There are many other methods besides waitForTime(long): waitForEvent(EventInput), waitUntil(BooleanInputPoll), waitUntilOneOf(BooleanInputPoll...), waitUntilAtLeast(FloatInputPoll, float), and waitUntilAtMost(FloatInputPoll, float).
Let's pretend that we have a limit switch that will be triggered when the robot hits a wall, on digital I/O port 2. Let's get a BooleanInputPoll for this (declared outside the autonomous mode):
final BooleanInputPoll limit = Igneous.makeDigitalInput(2);
Now, instead of the waitForTime call, let's say:
waitUntil(limit);
Try it in the emulator! Enable it in autonomous and press the 2nd digital I/O button.
Now, what if we want to wait for the limit switch, but stop anyway after five seconds if we don't hit it?
We unfortunately can't use waitForTime in conjunction with another method, but we can use multiple BooleanInputPolls. We have two ways to do this.
Way one: let's first calculate when we want to time out:
float timeout = Utils.currentTimeSeconds.get() + 5;
Next, let's see how we'd use this as a simple wait:
waitUntil(FloatMixing.floatIsAtLeast(Utils.currentTimeSeconds, timeout));
Try that in the emulator instead of the limit switch. Notice how it works like the original waitForTime.
Now, let's combine this with the limit switch:
waitUntilOneOf(limit, FloatMixing.floatIsAtLeast(Utils.currentTimeSeconds, timeout));
(Note that waitUntilOneOf() can take any number of arguments.)
Now try that. You should be able to either press the limit switch or wait five seconds and the motors will stop.
What if we want to continue our autonomous mode one way if we hit the switch, and another way if we time out?
It turns out that waitUntilOneOf() returns the index of what eventually caused it to end. This means that we can write:
if (waitUntilOneOf(limit, FloatMixing.floatIsAtLeast(Utils.currentTimeSeconds, timeout)) == 0) {
// The limit switch was pressed
} else {
// The limit switch wasn't pressed.
}
So, we could make it drive backwards for one second if it hits the limit switch:
if (waitUntilOneOf(limit, FloatMixing.floatIsAtLeast(Utils.currentTimeSeconds, timeout)) == 0) {
leftDriveMotor.set(-0.5f); // Notice how we need to put an 'f' symbol after the number because it's a float instead of a double.
rightDriveMotor.set(-0.5f);
waitForTime(1000);
}
Try that in the emulator!
Since the code in these methods is normal Java code, you can do (almost) anything that you could normally do.
The one important thing to watch out for is any infinite loop that doesn't have some sort of wait*() method in it - the CCRE needs one of these to be called often for the autonomous mode to function properly.
You can also use InstinctModules for uses other than autonomous. This will be documented in a future tutorial!
package team1540;
import ccre.channel.BooleanInputPoll;
import ccre.channel.BooleanStatus;
import ccre.channel.FloatInputPoll;
import ccre.channel.FloatOutput;
import ccre.ctrl.DriverImpls;
import ccre.ctrl.FloatMixing;
import ccre.igneous.Igneous;
import ccre.igneous.IgneousApplication;
import ccre.instinct.AutonomousModeOverException;
import ccre.instinct.InstinctModule;
import ccre.util.Utils;
public class MyFirstRobot implements IgneousApplication {
public void setupRobot() {
final FloatOutput leftDriveMotor = Igneous.makeTalonMotor(1, Igneous.MOTOR_FORWARD, 0.1f);
final FloatOutput rightDriveMotor = Igneous.makeTalonMotor(2, Igneous.MOTOR_REVERSE, 0.1f);
FloatInputPoll leftDriveStick = Igneous.joystick1.getYChannel();
FloatInputPoll rightDriveStick = Igneous.joystick2.getYChannel();
DriverImpls.createSynchTankDriver(Igneous.duringTele, leftDriveStick, rightDriveStick, leftDriveMotor, rightDriveMotor);
BooleanStatus shifter = new BooleanStatus(Igneous.makeSolenoid(1));
shifter.setTrueWhen(Igneous.joystick1.getButtonSource(1));
shifter.setFalseWhen(Igneous.joystick2.getButtonSource(1));
shifter.toggleWhen(Igneous.joystick1.getButtonSource(2));
Igneous.useCompressor(1, 1);
final BooleanInputPoll limit = Igneous.makeDigitalInput(2);
Igneous.registerAutonomous(new InstinctModule() {
protected void autonomousMain() throws AutonomousModeOverException, InterruptedException {
leftDriveMotor.set(1);
rightDriveMotor.set(1);
float timeout = Utils.currentTimeSeconds.get() + 5;
if (waitUntilOneOf(limit, FloatMixing.floatIsAtLeast(Utils.currentTimeSeconds, timeout)) == 0) {
leftDriveMotor.set(-0.5f);
rightDriveMotor.set(-0.5f);
waitForTime(1000);
}
leftDriveMotor.set(0);
rightDriveMotor.set(0);
}
});
FloatMixing.setWhen(Igneous.startDisabled, leftDriveMotor, 0);
FloatMixing.setWhen(Igneous.startDisabled, rightDriveMotor, 0);
}
}
Next: [The Logging Framework]([Tutorial 2.1] The Logging Framework)