INTO THE DEEP My Second Autonomous Program

This is a tutorial on creating your first FIRST Tech Challenge (FTC) autonomous program for the INTO THE DEEP 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.

INTO THE DEEP Program Tutorials

Prerequisites/Assumptions

This tutorial assumes:

  • a robot with two driving wheels, possibly a basic robot from the FIRST robot building resources page. These are sometimes called pushbots.
  • some way to push samples around. This robot has a square opening at the front into which a sample can fit.
  • A webcam is not required for this program.
  • The robot is assumed to have an active config file with motor names “left_drive” and “right_drive”. Adjust the motor names in the program if you’ve configured the motors with different names. Sample programs for two wheel robots will use left_drive and right_drive so may be easier to rename them in the config.
  • The motors require the encoders cables to be connected for each motor.
  • Your robot has a Control Hub so that you have access to an IMU (Inertial Measurement Unit).
  • some familiarity with Blocks, possibly a Blocks tutorial.

You can probably follow along even if you’re new to Blocks, however this tutorial doesn’t explain how to program in Blocks.

This is a much more complex program than the first program because it makes use of motor encoders and an IMU and has more steps in it.

Plan

Have the robot drive push a sample into the net zone and then park in the observation zone. This is a program can only be used on the Red Alliance and from a starting position near the red net zone. It will score 2 points for the sample and 3 points for parking.

The robot will follow the paths shown by the green line so it can avoid our alliance partner who we assume is parked and won’t move in AUTO.

Competition Manual Section 11.3 Pre-Match rule G303 indicates robots must start touching the Field wall and not be in the net zone or observation zone.

We COULD try and to this by moving only be time or motor encoders. The problem with moving by time is that it’s not very accurate and the distance the robot moves gets less as the battery runs down. The problem with moving by motor encoders only is that wheels will slip and skid during turns, so the turns are not very accurate either.

However, if we use the IMU for turning we can point the robot fairly accurately, and if we use motor encoders to drive straight we can move set distances fairly accurately. Using both types of sensors allows us to be more accurate.

We’ll start the robot in a fixed starting position with the front and rear wheels of the left side of the robot pressing against the wall and the front of the robot aligned with the edge of tile F2 closest to the red net zone. This fixed starting position means we don’t need to carefully align or point the robot every time we set up.

The steps this program will need are:

  1. use the IMU to turn away from the wall and point at the net zone
  2. drive forward into the net zone
  3. back up leaving the sample in the net zone
  4. turn to point towards the sub
  5. drive towards the sub and stop on tile E2
  6. turn to point towards the back of the field and stop on tile E5
  7. turn to face the wall
  8. drive forward and park on tile F5.

DeepAuto2

1. Start by connecting your laptop to your robot and starting the Block programming tool. Assuming you’ve connected your driver station to the robot, you can look at the Program and Manage page on your driver station. That will show you the network password for the robot and the IP address. The default password for a control hub is “password” and the IP address to use to connect to the robot is http://192.168.43.1:8080.

2. Then click on the Create New Op Mode button to create a new Op Mode and set the Op Mode Name to DeepAuto2, or whatever you want. Also select the Sample BasicOpMode.

3. Verify that you created the new op mode. You should see your newly created op mode opened for editing in your web browser’s main screen. You’ll probably have to change the mode of the program from TeleOp to Autonomous.

4. Check that the mode of the Op Mode says Autonomous, it’s beside the OpMode Name. You can also click the Show Java checkbox and close the window that shows the Java code for these blocks. The Java code can be interesting to look at, but it just takes up screen space if you don’t know Java.

5. Set up the initialization section as follows:

  • Reverse the direction of the left motor so that positive power will spin both wheels forward.
  • We initialize the motor encoders with a STOP_AND_RESET_ENCODER.
  • It turns out I didn’t use the myElapsedTime variable, you could add telemetry to each step and report on the current runtime of the program as it is executing.
  • Then initialize the IMU. This requires the orientation of the control hub on the robot. We’ll assume it is flat (so the logo direction is UP), and for this robot, the USB ports are facing RIGHT. Adjust as needed for your robot.
  • Then we use Telemetry to indicate the program is ready to start. Note: the IMU does take a little time to initialize.

6. Delete the Repeat While block, we’ll add our own series of steps.

7. Add the step 1 blocks to do a small turn away from the wall (about 10 degrees) so the robot points towards the red net zone.

  • You should reset the IMU Yaw value in case the robot was moved after INIT but before the START button was pressed.
  • You could skip the MyElapsedTimer reset block. I had that in my program to begin with, but decided that runtime wasn’t too useful and I would rather know what STEP of the program is currently running.
  • We start by applying positive power to the left motor and negative to the right motor, this will cause the robot to spin clockwise away from the wall. We also set the motor mode to RUN_WITHOUT_ENCODER since we’ll control the turn with the IMU.
  • We then enter a Repeat While loop that checks the opMode is still active (you haven’t pressed the STOP button on the driver station), and that the IMU Yaw value greater than -10. The Yaw value will start near Zero after an IMU reset, but as the robot turns clockwise the Yaw values will turn negative and get increasingly negative. We want to keep looping until the Yaw value is less than -10.
  • Inside the loop we have Telemetry indicated the current STEP and what the current IMU Yaw value is.
  • When the Yaw value get’s below -10 the loops exits and we set both motor power levels to zero.

8. Add a step to drive forward using motor encoders, it will look something like:

  • STOP_AND_RESET_ENCODER as we want both encoders to reset to zero.
  • Set a target position of 1800, this will depend on your motors and wheel size. For this robot this is a move of just over one foot.
  • We set power to both motors of 0.3, we keep the value low to avoid wheel slip on the tiles.
  • We then set the mode to RUN_TO_POSITION, this starts the motors running.
  • We enter a repeat loop while we wait, we check that the opMode is still active and that each motor is still busy.
  • Inside the loop we report the current STEP and the current position of one of the motors along with the target position
  • There’s a disabled sleep block, which can be handy to add to your programs as you develop them so there’s a clear pause between one step and the next. That can help you decide if the STEP turned enough or drove far enough.

9. Step 3 is to back off leaving the sample behind.

  • This is basically a copy of Step 2, except we DO NOT reset the encoders.
  • We can set the target to 1000 (which is less than the current target of 1800). This will cause the motors to run backwards.
  • NOTE: when running using encoders always supply positive power to the motors, the RUN_TO_POSITION will take care to apply the correct motor power to reach the target position.

10. Continue with STEP 4, turn to face the sub. This will be a clockwise turn, so we’ll turn from about -10, to about -140. this is the same as Step 1, except we add a STOP_AND_RESET_ENCODER block at the beginning of the step. See STEP 4 in the full program.

Note: Yaw is a normalized value which varies between -180 and +180. That can cause problems when you exceed +180 or -180, suddenly the Yaw reported will jump. eg. if we keep going clockwise the Yaw will go from -170 to -180 and then jump to +180 and then start decreasing to +170, +160, etc. Something to watch for later in our program.

11. STEP 5 is drive towards the sub. Copy Step 2 to drive with motor encoders. A Target Position of 3000 worked for this robot. See STEP 5 in the full program.

12. STEP 6 is to continue turning clockwise an face the rear wall of the field. Copy Step 4 and set the IMU target to -175.
NOTE: we do not use or exceed -180, because the reported Yaw from the IMU will jump from -180 to +180 which makes it harder to program. See STEP 6 in the full program.

13. STEP 7 is a long drive towards the rear of the field. Aim to stop on Tile E5. A Target Position of 4000 worked for this robot. See STEP 7 in the full program.

14. STEP 8 turn to face the observation zone. Note: our current IMU Yaw value (or heading) is about -175, if we keep turning clockwise the value will jump from -180 to +180 which makes our Repeat While loop hard to program. Let’s call an IMU.resetYaw block which resets the Yaw to zero, and then do a -90 clockwise turn from where the robot currently is.

15. STEP 9 drive forward to tile F5 in the observation zone. A Target Position of 750 worked for this robot. See STEP 9 in the full program.

16. STEP 10 stop all motors and provide feedback that the program is complete. Note: when the program ends any Telemetry messages are removed from the driver station, so we add a Sleep for 2 seconds.

17. Save your op mode. You should see the message “Save completed successfully.” displayed for a few seconds.

Here’s the completed program.

11. Run it and see what happens. The robot should follow the path as indicated in the diagram:

By adjusting the amount of time (or the power level) you can control how far the robot moves. Note: this type of movement is not very accurate and the distance moved will vary depending on your battery level. However, we don’t need to be very precise as we just need to stop anywhere in the zone.

Your robot may vary depending on what motors you are using and the size of the wheels you are using. Adjust the power levels and motor encoder targets appropriately.

If you don’t have a field for testing, you’ll need to have a space that’s about 12 feet long and 4 feet wide (which is two tiles wide and six tiles long).

Here’s the robot running this program. One thing you might note is that the rear wheels are actually covered in one inch red gaffer tape (the same tape used on the field). This was because the rubber on the wheels on this robot are smooth and shiny and I had noticed they slipped a lot on these tiles. With cloth tape as a wheel cover, traction was a little better.

DeepAuto2 Scoring a Sample and Parking

Next Steps

You should have a look at last year’s tutorials on motor encoders and using the IMU.

This year the RobotAutoDriveByGyro program is available as Blocks sample program and I recommend you create that program learn about working with encoders and the IMU and using functions.

We’re reaching the limit of what a simple pushbot can do in AUTO. But we could do a little more, like push up to three samples into the net zone and score a level 1 ascent. Check out the DeepAutoAscend program.

Getting Help

It is often possible to use Google (or other search engine) to get help or solve problems. There are lots of resources online. If you’re still stuck you can ask for help here.

You can also check out last year’s tutorials on creating autonomous programs for CENTERSTAGE.