Combining Commands – V3

Now that we have commands to drive and turn let’s see how we can combine them to create more complex commands. Let’s create a new command that will drive the robot around a certain path.  The first step is to create a new function named driveCourse which returns a Command as before:

Now we are going to combine the driveStraight and turn commands to a single command. To start, let’s say we want to drive forward 20 inches and then turn left by 90 degrees, we would add the following lines to the function:

The coroutine.await(…) call tells the schedular to run the command returned by driveForDistance(…) and wait until it has completed before continuing. Also notice how I placed the built in wait(…) command between the driveForDistance() and turn(). The wait(…) command does nothing but wait for a specified time. Adding a small wait between the commands will increase the accuracy of the turns. Transitioning directly from driving to turning can cause errors like wheel slippage which will throw off our turn calculations. I am also using variables turn90, driveSpeed, turnSpeed, and wait rather than specifying the numbers directly in the functions. I am doing this since we will be adding multiple commands that use these numbers and defining them all in one place will make it easier for us to adjust the numbers if needed.

Now modify the TeleopOpMode class so that we run this new command when we press our test button 6.

Now deploy and run your program and verify that it drives forward 20 inches, turns left and then stops. Adjust the turn90 variable so that the turn is correct if needed.

A question about requirements

If you have been paying attention, something here might seem odd. driveCourse is created with run(…), so it requires the DriveMechanism. But driveForDistance and turn also require the DriveMechanism. We learned that only one command can require a mechanism at a time, so why doesn’t starting driveForDistance cancel driveCourse?

The answer is that the scheduler knows driveForDistance was started by driveCourse using await. It is a child of driveCourse, and a child command is allowed to use the mechanisms its parent requires without interrupting the parent. So driveCourse holds the DriveMechanism for its entire run and lends it to each child in turn.

To see why this matters, try an experiment. In driveCourse, change run to Command.noRequirements:

Now driveCourse doesn’t require anything itself. Each child requires the DriveMechanism only while it is running and releases it when it finishes. Run the course again, and during one of the half-second waits, move the joystick. You will find that you can steer the robot! Between the steps nothing requires the DriveMechanism, so the scheduler starts its default command, arcadeDrive. (If you don’t touch the joystick you probably won’t notice anything, because arcadeDrive just keeps the motors stopped.)

Change it back to run when you are done. A routine like this one should normally hold its mechanism for its whole run, so that nothing can sneak in between its steps.

Now let’s change your program so that the robot will drive in a rectangular pattern 20 inches by 10 inches. In principle this should bring your robot back to the starting position. After you have completed and tested your code, compare it to my solution below.

.

.

.

.

.

.

Note that in my case I needed set the turn90 to 86 degrees instead of 90 in order to get the robot to drive the correct path. You will probably find that it was very hard to set the speed and turn parameters correctly to bring the robot back to the exact starting position. In fact, you probably saw that running the same program twice in a row actually produced a slightly different outcome. This is because there are a number of things that go on which introduce slight errors (such as wheel slippage), and these errors accumulate over time and are different every time. You might find that if you slow the robot down, you can get more reproducible results, but you can never reach the point where it is always spot on.

The important take-away from this: This kind of ‘dead reckoning’ navigation is fraught with problems. The only way to overcome these problems is to use sensors (e.g. gyro, distance sensor, camera, etc.) on the robot that give you a way to determine its absolute position and orientation by observing and using the environment around it. This is how the competition robots determine their position on the field. We will also be using these more advanced techniques when you graduate to the SwerveBot.

Next: Using Sensors