There’s no Blocks sample program in the SDK for using encoders to move the robot. There is a Java sample program for encoder based movement so let’s make a Blocks copy of that program.
This is a tutorial on creating FIRST Tech Challenge (FTC) autonomous programs for the CENTERSTAGE game.
Visit the FTC docs page on Creating Op Modes in Blocks if you need help getting started and to create your first driver controlled program.
The encoderDrive function from this program will be used to create a program to put the purple pixel on a spike mark.
Autonomous Blocks Program Tutorials
The Java program has an encoderDrive function and this program will use that function to create the following movement path for the robot:
- Drive forward for 48 inches
- Spin in place, about a quarter turn clockwise
- Drive Backward for 24 inches
This is a good example of using encoders to move around. It has a Blocks function that moves the robot according to some input parameters, and then calls the function three times to perform the required movements. These types of controlled movements are needed to place pixels on the spike mark or the backdrop.
This tutorial assumes you’ve got some familiarity with Blocks and that you have a robot configuration in your driver station. Ideally you’ve completed a Blocks tutorial. This tutorial doesn’t explain how to program in blocks.
This tutorial assumes you have a robot with two driving wheels, normally referred to as a tank drive or pushbot drive.
Your robot must have encoders on the motors and an encoder cable must connect the motor to the Rev Control Hub (or expansion hub).
A webcam is not required for this program.
1. Copy the TestEncoders program and call it RobotAutoDriveByEncoder_Blocks.
2. Create some variables that in the initialization area. Add them after the block to change the direction of the left motor.
A common naming convention from Java programming is to name variables in camel case such as myElapsedTime. But variables that don’t change are called constants in Java and the convention is to spell them with all capital letters and underscores to separate word parts. COUNTS_PER_MOTOR_REV is constant value that doesn’t change later in the program. COUNTS_PER_INCH is set to a calculated value, but is not changed later. myElapsedTime is a variable that changes in the elapsedTime function. This naming convention can be helpful in larger programs when you start to get lots of variables and constants.
3. Insert a STOP_AND_RESET_ENCODER block for both motors and a RUN_USING_ENCODER block after the set TURN_SPEED block. Also a telemetry addLine block with a create text with block that display’s both motor encoder positions. We want to reset the encoder position to zero, and then indicate that will be using encoders. The telemetry line isn’t strictly required, but copy this block and add it to the encoderDrive movement loop later. Having the driver station display the values at zero and then seeing the values increase when you press start is a nice to have.
4. Create a new function and call it encoderDrive. This function will be called every time the robot moves. Block functions are a great way to group together a set of commands to use again and again. You can even pass parameters to the functions so that they can use those parameters inside the function.
- encoderDrive has four parameters: speed, leftInches, rightInches, timeoutS where:
- speed is a motor power level from -1 to +1,
- left and right inches are how far we want the left and right side wheels to drive, and
- timeoutS is the amount of time in seconds after which the function will exit if the robot hasn’t hit the target positions.
- A timeout means that if a wheel get jammed due to a robot collision or the whole robot is stopped because it is blocked, then the wheels might not turn and the loop inside the function would never exit. This could also happen if you supply too low a power level and the motors are unable to overcome the static friction and inertia of the robot when it is standing still.
5. Add some comments and and If logic block with a call opModeIsActive test.
5. Inside the If block Add a comment and some blocks to Determine new target position to the start of the new function
6. Then add setting the mode to RUN_TO_POSITION
7. reset the timeout time and start the motors moving
8. Insert a Repeat loop and keep looping while the robot is still active. This has a complicated While test. There are three nested and logic tests. All must be true for the loop to continue. The first test is opModeIsActive, the second test checks for a timeout, the last check is to confirm that both motors are still busy.
Note: this While loop will exit as soon as the first motor reaches it’s target. That works if both motors are running to the same target. If the left and right distances are different, change the isBusy AND comparison to an OR comparison. That way the program will continue running until both motors have reached their target.
9. Inside the loop insert some comments and some telemetry to display while waiting for the motors to reach their targets.
10. After the repeat loop stop all motion and reset the run mode.
Note: there is an optional sleep block at the end of the function. That might help in testing multiple moves to make it clear when one movement stops and the next begins. You may want to disable the sleep block when you’ve finished testing.
That completes the encoderDrive function. The complete function is shown here:
11. Now let’s set up the main loop. If you copied the TestEncoders function you’ll probably end up deleting almost all the blocks inside the IF block. Add some comments and then call the encoderDrive function 3 times as follows:
Here’s the complete RobotAutoDriveByEncoder_Blocks program, click the thumbnail to see the full image. You may need to download the image and view the image on your laptop or PC to see the full program.
Here’s a video of the robot running this program. I ran the Java version of the program and the robot moved the same.
Experiment with this program before using the encoderDrive function in other programs.
- Verify the encoderDrive function moves correctly. Try disabling Step 2 and verify that the robot actually moves 48″ forward and then backs up 24″ and ends up 24″ from where it started.
- Try setting one motor to a different distance. Disable the 3rd step so we only do one move. Then maybe try leftInches = 48 and rightInches = 36.
- Then try setting rightInches to 0. How well does it turn?
- Try setting different DRIVE_SPEED and TURN_SPEED values.
Now that the robot can move more precisely, the next step is to try pushing pixels to the spike marks. You will need to learn to use TensorFlow to detect the white pixel on the randomized spike mark, or have another way of identifying which spike mark to use.
Check out the CENTERSTAGE TFOD-pixel Autonomous Program that uses TensorFlow and the encoderDrive function to correctly place the purple pixel on a randomized spike mark.
Check out the other Blocks sample programs: