CENTERSTAGE TFOD-pixel Autonomous Program

The autonomous program we have developed so far can score from all four starting positions, but are limited to 11 points. How can we score more points?

There are 10 points available if we can place the purple pixel onto the randomized spike mark. If we then park in the backstage area we can earn 15 points. If your robot can place a pixel on the backdrop we could later add to this program and score another 10 points. If we train a TensorFlow model to detect a Team Prop, we could earn 10 bonus points for the spike mark and 10 more points for a pixel on the backdrop.

This program will start in the rear blue alliance starting location.

Prerequisites/Assumptions

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

Plan

The plan is to started aligned with the rear edge of tile A4 with rear of the robot flat against the field wall. We then have a complicated multipart plan with many steps and some tests along the way that change where we go next. We’ll develop the program in stages as there are too many unknown details at this stage.

The basic plan

• move closer to the left spike mark (as looking at the marks from the robot position);
• then use TensorFlow to check if there is a white pixel there;
• If there is a pixel, drive forward and drop off the purple pixel on the left mark and turn and drive to the backstage corner tile A6;
• If there is no pixel on the left mark, we turn and check the center spike mark. If the pixel is on the center mark, drive forward and drop off the purple pixel and then turn and park on tile A6;
• If the pixel was not found so far we will assume it is on the right side spike mark. Turn toward that mark, drive forward and drop off the purple pixel on the right mark and then turn and park on tile A6.

CENTERSTAGE TensorFlow

TensorFlow is included in the SDK and is updated each year to detect some game element. TensorFlow can do many different things, but in FIRST Tech Challenge it is used to detect objects in a webcam image. This year TensorFlow can detect a pixel on the field and you can also train a new TensorFlow model to detect your Team Prop.

In this FTC Docs article about CENTERSTAGE TensorFlow they describe TensorFlow and how it can be used this year. They describe one of the limitations about using TensorFlow on the pixel from the starting position and mention how your robot needs to move closer to the pixel and ideally have a webcam mounted somewhat high up on the robot that looks down on the pixel. See the examples in the article.

So that is why we have the robot moving forward and then trying to detect the pixel. Because we’re so close we cannot see all three spike marks, so we end up having to point the robot at each spike mark so we can check it with TensorFlow.

FTC Docs has an article on creating a TensorFlow Blocks program. It’s worth reading that article to get a sense of how to use TensorFlow and some of the different ways you can set it up. Basically you initialize TensorFlow and then later in your program you can call a TensorFlow block to get a list of any objects TensorFlow has detected.

The Blocks programming environment includes a sample Blocks program called ConceptTensorFlowObjectDetectionEasy. You should create a new program based on that sample. I called my program TFOD-easy. We will use that program to check what our webcam can see as it moves about the field and whether or not it can detect the pixel. We’ll use the initTfod function in that program in our new program.

Adjust Webcam Orientation (if needed)

Start by placing the robot on the field and starting the TFOD–easy program. If you initialize the program and then press the three dots on the driver station you can select Camera Stream which will show you the webcam view on the driver station. If TensorFlow was initialized it will show any object detections. I found that TensorFlow did NOT work well with my webcam at least where it was placed. If the robot was too far, or the image of the pixel was such that we were not looking down on the pixel, then TensorFlow might not detect the pixel.

As mentioned in the FTC Docs CENTERSTAGE TensorFlow article I found I had to get close to the pixel but even then it didn’t always detect the pixel. I ended up having to change how the webcam was mounted so that it was actually pointed downwards. Initially the webcam was mounted flat parallel to the floor and though it was at the top of the robot, this robot is not very tall. I had assumed that the up/down swivel of the webcam would be enough, it wasn’t. The down angle after the change is not quite 45 degrees, it might be 30 degrees. By the way, this robot has a Logitech c270 webcam.

With the robot about one foot from the pixel I then got reliable TensorFlow object detections. I think TensorFlow needs to see the hole in the middle of the pixel and the raised ribs on the top surface of the pixel and the pixel needs to be fairly large in the image.

Note: as seen in the image above, the webcam resolution is 640×480. This is the default for TensorFlow and April Tag vision processing. 640×480 is the resolution you get when using the Easy vision processing initialization blocks. This is fine for most uses. In fact, using a high resolution webcam could be a problem with TensorFlow. As mentioned in FTC Docs the webcam image is downscaled from the high definition webcam image to one that is only 300×300 pixels.

Stage One TFOD-pixel1

We going to develop this program in stages. In Stage One we will call our program TFOD-pixel1.

We’ll start by moving the robot so that we it could detect the pixel on each of the spike marks, but we’ll only be moving the robot. Later stages will add TensorFlow pixel detection and movements to place the purple pixel. We’re going to create a series of steps in our program, one step per robot action.

The robot will be placed on tile A4 with the rear of the robot flat against the field wall and the rear tire at the backstage tile edge.

1. Let’s start by creating a program called TFOD-pixel1 copied from our RobotAutoDriveByEncoder_Blocks program.

2. Modify the main loop by disabling the second and third blocks. Don’t delete them, we’ll use them later. Change the first step to drive forward 16 inches. That distance was determined by placing the robot at a reasonable distance for pixel detection and using a tape measure. We’ll set the leftInches parameter to 16. Set the rightInches parameter to 19, this will cause a slight turn to the left. Set the timeout to 3, it’s not far to move. In addition, we’ll delete the step one comment and replace it with a Step 1 telemetry addData and a telemetry update.

We will add a telemetry message to every step, not only are they like comments in a program, we will also see those messages on the driver station. That can help when things go wrong.

3. Modify the encoderDrive function to change the left_drive.isBusy and right_drive.isBusy comparison to OR from AND. We want the program to keep running as long as both motors are still busy. By having our right motor drive further, after we have driven 16 inches the left motor will stop and the right motor will keep driving which will cause our robot to turn to the left.

Locate the two telemetry addLine and the telemetry update blocks in the encoderDrive function and disable them. You could delete them, they are not likely to be used. In the initialization function disable or delete the telemetry addLine “Starting at …” block.

4. Change the DRIVE_SPEED variable and set it to 0.3, I tried the 0.6 value and found the robot drove further by an inch, probably had hard time stopping. With 0.3 I think I was within a half inch of my target 16″ of movement.

Save the program and try running it. It should move forward and point the webcam at the spike mark.

After the program runs, leave the robot where it is. Use your driver station to select and run the TFOD-easy program you created earlier. Initialize the program and then select the three dots and the Camera Stream and check what the webcam is seeing. The pixel should be fully visible and TensorFlow should have drawn a bounding box and labeled it a “Pixel”. If your webcam is not on the center of your robot your image maybe different, that is probably ok as long as the pixel is visible and it is detected. The whited out areas at the top and bottom are NOT being used by TensorFlow, considered them cropped out by TensorFlow. This is due to the aspect ratio of 640×480 being different than the c270 webcam.

You may have to make a variety of changes at this point. You may have to change how far the robot moves or how much of a turn it makes in Step 1. You may even have to change how the webcam is mounted. The robot needs to be able to detect the pixel and ideally your purple pixel pusher is pointing at the center of the left spike mark.

Note: Step 2 is where we will later add a TensorFlow test to our program, but for now let’s assume that check failed. Let’s add a move/turn that points us at the center spike mark.

5. STEP 2 position robot to check center spike mark. This will require a slight turn to the right, and we want to move a little closer in order to use TensorFlow. Let’s enable the second encoderMove block and set the leftInches to 7 and the rightInches to 2. We’ll move a couple of inches closer and turn to the right. Later I adjusted the leftInches to 7.5.

The telemetry will be helpful since we’ll end up with six moves in stage one, and many more to complete our autonomous program. Save and run the program. The robot should move and point at the left spike mark, then move and point at the center spike mark.

Now run the TFOD-easy program and check the Camera Stream.

Adjust your program as needed. Ideally your pixel pusher is pointing at the center of the center spike mark and your webcam is showing a detected pixel on the center spike mark.

Note: This is where we would add a second TensorFlow object detection, but let’s assume that also failed.

This mean that we are pretty sure the pixel should be on the right spike mark. We could try and detect the pixel on the right spike mark, but that will take time. Therefore we will just assume the pixel is on the right mark and go there. This also handles the scenario where TensorFlow is not detecting the pixel, the robot will not get stuck waiting for TensorFlow and the robot will still score for parking backstage.

6. STEP 3 turn the robot and point at the middle of the right spike mark. Re-enable the third step and and set the leftInches to 6.25 and the rightInches to 0. (I started with 12, and tried several times to get closer to want I wanted). This should turn the robot to the right and point the robot so we can place the pixel in the center of the right spike mark. Save the program and run it. Adjust until the robot is pointing at the center of the right spike mark.

7. STEP4 is to move and stop with the purple pixel on the right spike mark. Measure the distance needed to place the pixel on the spike mark and use that number in a call to encoderDrive. Duplicate one of the calls to encoderDrive and place it as our fourth movement block. Set the leftInches and the rightInches to that number, my measurement was 10.5” which I later changed to 10 inches. Save the program and run it. Adjust this step, or the turn in the prior step, until the purple pixel is placed on the center of the right spike mark.

With the purple pixel on the spike mark, this robot now needs to back up straight to drop off the pixel. Then we want to turn so that the robot can go park backstage.

8. STEP5 back up and turn to align with the corner tile. Add a long encoderDrive move of -30 inches so that the robot moves backwards from the spike mark and leaves the purple pixel behind. Set the rightInches to -36 and leftInches to –30 so that the robot ends the movement with a turn so that the robot is lined up to drive into the corner tile. Save the program and run it. Verify the turn leaves the robot aligned so it has a straight path to the corner tile.

Note: I actually ran into a timeout problem using encoderDrive. I had been duplicating the previous blocks that had DRIVE_SPEED = 0.3 and timeoutS = 3. The movement ended before it reached the target position and did the turn. I ended up increasing the timoutS to 5, but I also increased the speed to 0.5, the robot can go faster when its going longer distances, it doesn’t have to be precise for longer distances. At this point we’re just trying to park backstage.

9. STEP6 move the robot onto the corner backstage tile. Measure how far to get backstage and add a move for that distance. In my case it was 32 inches, so I set -32 since the robot is moving backwards.

That completes our Stage One steps. Our program actually places the purple pixel but it only would be correct one-third of the time. But it should also score for parking backstage.

Here’s a video of the program in action. So if the pixel was on the right spike mark this program would score 10+5=15 points.

Here’s a copy of showing the driver station telemetry while this program is running.

Here’s a copy of the program.

Stage Two – Add TensorFlow Object Detection for Left Spike Mark

Now that we have the starting moves for the robot we should add TensorFlow Object Detection of a pixel on the left spike mark. If a pixel is found we’ll drop off the purple pixel on the left spike mark and park the robot backstage.

1. copy the TFOD-pixel1 program and call it TFOD-pixel2. We’re going to make a lot of changes so let’s start a new program. Also, we may use this TFOD-pixel1 to create new autonomous programs that start at other locations.

Actually we probably won’t reuse TFOD-pixel1, but it’s good programming practice to save your work before you make a major change. That way if it doesn’t work out, you can always go back to the older program. Later on you can delete programs you don’t need.

2. create a new function called initTfod as follows. This is taken from the initTfod from the TFOD-easy program and makes use of easyCreateWithDefaults. The IF block has been removed and I only the set myVisionPortal for a webcam is since I’m using a webcam. We need two blocks (and some comments), basically a call to create a TensorFlow processer and save it in a variable called myTfodProcessor. Then next block creates a Vision Portal object which we initialize with our myTfodProcessor and save the Vision Portal in a variable called myVisionPortal. There are many many things you can do with Tfod Processors and Vision Portals, but we don’t need to get too complicated for our program.

If you’re using a phone for a robot controller, copy the vision portal block that uses the phone camera. Add comments as needed.

2. Call the initTFOD function in our initialization section after the motors are initialized and before the initialization complete message.

If we were to save and run this program and initialize, the Camera Stream would be enabled and show any detected pixels.

3. Create a new function called detectPixel, use the function block that has a return value. We can add a block to do the TensorFlow detection and then check how many objects were detected. Since we are not driving at the same time as we are using TensorFlow we can make use of the myElapsedTime variable to have a TensorFlow timeout, just like we have a driving timeout. Reset the myElapsedTime variable. Insert a while loop and add three second timeout test and also a test to see if we have at least one TensorFlow object detection. Test if the length of the myTfodRecognitions is greater than 0. Inside the loop we can add a sleep 250ms. Then we check TensorFlow again by getting the list of detections again. It may take a second or two for TensorFlow to detect an object. Finally, we test the length of the myTfodRecognitions and set pixelFound to true/false. We then return that value. The detectPixel function should look like the following:

4. Insert an IF block after the STEP 1 move, then add a Telemetry command in front of the IF and an update block, the telemetry should say “1a – detect left spike mark“. Set the IF to be a call to the detectPixel function. That function will return a true or false value which we can use directly in our IF block. Inside the IF just add a telemetry and sleep command as follows.

6. We’ve done a lot so far. Let’s save the program, put a white pixel on the left spike mark and run the program. You should get a Telemetry message that the pixel was found. If not, stop the program and try running the TFOD-easy program and confirm that the webcam is able to see the pixel and TensorFlow is able to detect it.

7. Add Step 1b – Inside the IF portion, we now need to move so that we place the purple pixel on the left spike mark. Measure how far the purple pixel needs to be pushed, I measured 11 inches. Change the telemetry inside the IF to say “1b – pixel found moving to left spike mark“. Then add an encoderDrive function to move forward 11 inches. Save the program and run it. It should stop with the purple pixel on the mark.

Test this and verify you can drop off the pixel on the left spike mark. Adjust the distance moved in 1b, or the amount of turn in step 1 until the purple pixel is well placed. I’m going to say the pixel in the image above is good enough.

8. Add Step 1c – Now we need to back up and drive to the corner backstage tile. Add an encoderDrive call to back up -6 inches straight so that we leave the purple pixel on the mark. Then we can start the turn. Let’s try -6 inches for the left motor and -18 inches for the right motor. Add telemetry blocks to display 1c – back up and turn to face corner.

Save and run the program. I adjusted rightInches to -24 eventually so that the the robot was lined up to back up onto the corner backstage tile.

Once step 1c is working, measure how far the robot has to go to park backstage. I measured -45 inches.

9. Add Step 1d – add telemetry blocks to display “1d – back up and park backstage” and a call to encoderDrive to move -45 inches for both motors. Save and run the program. Verify the robot parks backstage on the A6 corner tile and does not hit the backdrop. Ideally the robot parks well inside the the tile, maybe even touching the back wall. That will leave enough room for your alliance partner’s robot to also park backstage in the corner (they just have to be touching the blue tape).

Here’s a video of the program running if the pixel is on the left spike mark.

Here’s a copy of the IF block with some comments. The Sleep 30000 command means that if we find the pixel on the left mark we can do all the 1a to 1d steps and then stop. There’s no need for complicated IF ELSE blocks.

Note: there is a requestOpModeStop block that will stop the opMode. Probably better than using Sleep 30000.

Next we can try and detect the pixel on the center spike mark and drop the purple pixel there.

Stage Three – Center Spike Mark

If the pixel is not on the left spike mark we need to check the center spike mark. We already have Step 2 that turns the robot to face the center spike mark. Add a detectPixel function call to see if the pixel is there.

1. Start by copying the TFOD-pixel2 program and creating a TFOD-pixel3 program.

Again we probably won’t reuse this program, but it’s good to save the work we’ve done so far.

2. Then add telemetry after Step 2 and display the message “2a – detect center spike mark“. Add an IF detectPixel block, then add a Sleep 30000 block inside the IF. We’re basically going to do something similar to the 1a-1d steps. You could duplicate that IF statement and move it here and then update as follows. If you do that, disable the b, c and d moves for now, we need to see that each step is correct and the actual b, c, and d moves are slightly different. The 2a and 2b steps should look like:

Save your program, place the white pixel on the center spike mark and run it. You should see the “2b – pixel found, moving to center spike mark” message. Then measure how far the purple pixel in your pusher is from being on the spike mark. In my case it was 12 inches.

3. Add an encoderDrive block after the Step 2b telemetry update block to move forward 12 inches. Save the program and re-run it. Verify the purple pixel is well centered on the center spike mark. Adjust the step 2b move or the Step 2 turn as needed until the purple pixel is well placed on the center spike mark. I adjusted my 2b move to 12.5.

Now we should back up and leave the pixel, then turn so we can line up on the backstage area. Something similar to Step 1c.

4. Add Step 2c – add an encoderDrive block. I think we want to back up a little more that the 1c step. Let’s try –12 inches for the left motor and -30 inches for the right motor. Add a couple of telemetry blocks to display 2c – back up and turn to face corner.

Save and run the program. Adjust if need you think it can drive straight back into the corner tile. Then measure the distance to the backwall and use that in another step. I measured 47 inches.

5. Add Step 2d – add an encoderDrive block with -47 for both motors. Add a couple of telemetry blocks to display 2d – back up and park backstage. Save and run the program. Adjust if needed until the robot is well parked on tile A6 (backstage blue corner).

Here’s a video the robot detecting the center spike mark, placing the purple pixel, and then parking backstage.

Here’s the completed TFOD-pixel3 program:

Create TFOD-pixel-blue-rear

We’re essentially done at this point. The robot already handles dropping the purple pixel on the right spike mark. We should copy the program from TFOD-pixel3 to TFOD-pixel-blue-rear and save it. This name is more meaningful as the program only works from the rear blue starting location.

Check for mistakes in the program and add comments if needed. For example, somehow I ended up using power values of 5 in steps 1c and 1d. Those values should be 0.5. I had noticed the robot driving quite fast towards the backstage area, but it doesn’t need to be that fast.

The sleep 30000 blocks should be replaced with a requestOpModeStop blocks.

You can also disable the Sleep 250ms block inside the encoderDrives function so that our autonomous program completes as soon as possible. We’d like to be finished moving around the backstage area before our alliance partner robot finishes checking their spike marks and starts moving backstage.

You could then download the TFOD-pixel1, 2 and 3 programs to a laptop/PC and save them somewhere and then delete from the robot. Though I ended up using TFOD-pixel1 to start testing use of TensorFlow to detect a Team Prop.

You should also download and save all your current programs, having a backup is useful in case you accidently delete a program or make a change that you later wish to undo.

Next Steps

1. Create a program that runs from the Red alliance rear starting position and park in the red backstage corner. It would be similar to this program, but with the turns reversed.
2. Create two more programs that run from the front starting positions. They would use similar program logic to detect and place the pixels, but then they have to navigate and park backstage. You probably know enough by now to do that. Back up into the tile near the alliance field wall and turn the robot so it can drive along the field wall under the rigging. We’ll assume our partner has moved out of the staring position. We could park in the corners for now, assuming our partner is parked elsewhere or that both robot might fit in the one tile (we just need to be over the front edge of the tile). Or you could plan on turning after you get close to the backstage area and park more toward the middle of the field.
3. If your robot is able to place a pixel on the backdrop, you could try creating a program that starts backstage, and after you place the purple pixel on the correct spike mark you attempt to navigate by using motor encoders to the backdrop and then drop off the yellow pixel in the correct left, center or right area of the backdrop to score the yellow pixel.
4. If that’s too much trouble, you could instead simply have the robot carry the yellow pixel and then drop it when the robot has parked backstage. A pixel in the backstage area is also worth points.

More Advanced Programs

1. For driving long distances and more accurate turns you could look at using the IMU sensor that is in a Control Hub. Check out the Blocks copy of the RobotAutoDriveByGyro_Linear.java sample program.
2. If you have a way to place pixels on the backdrop, you should learn to use April Tags so you can line up on the various April Tags on the backdrop and score the yellow pixel on the backdrop in autonomous. Check out the RobotAutoDriveToAprilTagTank_Blocks sample program.
3. Finally, you could train a TensorFlow model to detect a Team Prop. If you use a Team Prop you can earn 10 bonus points for the spike mark and another 10 for the backdrop. Here are our guidelines for creating Team Props, taking videos of the props for TensorFlow, and training a custom TensorFlow model.
4. With custom TensorFlow model you could modify the TFOD-pixel program to detect team props instead of the pixel and use that to place the purple pixel and drive backstage.
5. If you trained TensorFlow to recognize your Team Props from the starting positions, you could use a modified TFOD-prop program that starts by detecting the team prop on the randomized spike mark from the starting position and then starts driving.

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.