Apriltag controlled car

Using VIA Pixetto’s AprilTag Recognition to Create a Self-Driving Car

In this tutorial, we are going to demonstrate how to use the VIA Pixetto, an Arduino board, and the AprilTag identification function to control a self-driving car.

This consists of four steps: connecting the VIA Pixetto to a PC, configuring the VIA Pixetto and connecting it to an Arduino board with the Grove connector, editing the VIA Pixetto program with program blocks, and uploading the program to the Arduino board installed on the self-driving car.

Step 1

Connect your VIA Pixetto to your computer via the Micro USB B cable.

Step 2

Click the link below to download the AprilTag 16H5 image file and print it out:
http://cdn.viaembedded.com/Pixetto/Demo/AprilTags.pdf

Click the link below to download the Arduino library compressed file “SmartSensor.zip”:
http://mls.pixetto.ai/files/SmartSensor-1.0.9.zip

Step 3

Open the Pixetto Utility application and set the “Function” to
“AprilTag(16h5)”. The VIA Pixetto lens should recognize the printed AprilTag, and the label under the AprilTag will reflect the information given (in this case the numbers 0 and 1).

We can use this information to allow our car to make decisions about where it is going to drive.

Step 4

First, we need to connect the VIA Pixetto to the self-driving car.

Next, we need to connect the Grove with VIA Pixetto sensors.

Make sure to connect the Grove cable to the expansion board and to connect the expansion board to the Arduino development board.
Unplug the Micro USB cable originally connected to the VIA Pixetto, and use it to connect the computer and the Arduino development board mounted on the self-driving car.

Step 5

If you have not installed the Arduino IDE, please download it from the Arduino official website first:
https://www.arduino.cc/en/main/software
Next, open the Arduino IDE:

In the upper toolbar, select “Sketch” and click “Include Library” in the list, then select “Add to .ZIP Library”.

In the pop-up window, select the previously downloaded SmartSensor library compressed file.

Then select “Tools” in the upper toolbar, and select the serial port that displays the name of the Arduino development board in “Serial Ports”, for example: COM 5 (Arduino Uno)

Step 6

Import the required libraries for this program to use:

  • “SmartSensor.h” is used to return data from the VIA Pixetto..
  • “Servo.h” is a built-in library for controlling servo motors.
#include <SmartSensor.h>
#include <Servo.h>

Now, we are going to create several global variables and constants:

  • focal_length: type float, which represents the focal length of the VIA Pixetto lens (unit: pixel), with a value of 43.04.
  • tag_diag: type float, representing the actual diagonal length of AprilTag. Please measure the side length (unit: cm) of the printed AprilTag and multiply it by the square-root of 2 to calculate the diagonal length.
  • cam_diag: type float, representing the diagonal length of AprilTag imaged by VIA Pixetto lens (unit: pixel).
  • dist: type float, representing the distance between the VIA Pixetto lens on the self-driving car and the actual AprilTag (unit: cm).
  • lt_bool: type bool, assigned false.
float focal_length = 43.04;
float tag_diag = 12.5*sqrt(2); //actual length times sqrt2
float diag;
float dist;
bool turn_bool = false;

Next, we need to specify which pins will be used to connect the Arduino development Board and the VIA Pixetto, (0 and 1). We also need to declare a Servo type variable in order to be able to interact with the motors. We will initialise these later in the setup function.

SmartSensor ss(0,1);
Servo servo;

Now that we have have declared these variables, we must initialise them:

void setup() {
// put your setup code here, to run once:
ss.begin();
servo.attach(5);
servo.write(90);
analogWrite(5, 50);
}

In the function named attach, the argument of 9 is used, which is the Grove port number (for example D9) that the servo motor is connected to. We use the write function with the argument of 90 which tells the servo motor to turn 90 degrees so that the front wheels of the self-driving car face straight ahead.
We then use the analogWrite function to move the car forward at a speed of 50Hz. The first argument of 5 refers to the Arduino pin that is being written to.

Step 7

Next, we are going to write the rest of program in the loop function.
We can use an “if” statement to confirm whenever the VIA Pixetto recognizes AprilTag, then we need to assign values to the two previously defined global variables “diag” and “dist”. The VIA Pixetto can return information about the identified object in order to do this.
The Variable “diag” represents the diagonal length of the AprilTag imaged by the VIA Pixetto lens. The length of the diagonal image can be calculated using the detected side length of the AprilTag and the formula for calculating the diagonal (Pythagoras’ theorem).
We can then use the calculated diagonal image length, the known actual size of the AprilTag and the focal length of the VIA Pixetto to calculate the distance between the currently detected AprilTag and the lens.

void loop() {
// put your main code here, to run repeatedly:
if (ss.isDetected()) {
diag = sqrt(pow(ss.getWidth(), 2)+pow(ss.getHeight(), 2));
dist = (focal_length*tag_diag)/diag;
}

Then, add the control of the speed of the self-driving car to the bottom of the assignment in the upper two lines to keep the speed at 50Hz.

void loop() {
// put your main code here, to run repeatedly:
if (ss.isDetected()) {
diag = sqrt(pow(ss.getWidth(), 2)+pow(ss.getHeight(), 2));
dist = (focal_length*tag_diag)/diag;
analogWrite(5, 50);
}

Step 8

Now, we want to program the self-driving car to perform a specified action when the VIA Pixetto recognizes the AprilTag and returns a specific number: one for stop, one to turn right, and one to turn left.
Therefore, we require three conditional expressions, one for each action the self-driving car can perform. For instance, if the VIA Pixetto reads and AprilTag and it gives back the number 0, we would want the car to stop. Or, it needs to do something different, such as turn to the left or turn to the right.

if (ss.getTypeID() == 0) { //if AprilTag # is 0
} else if (ss.getTypeID() == 1) { //if AprilTag # 1
} else if (ss.getTypeID() == 2) { //if AprilTag # is 2
} else {
}

If we want the self-driving car to stop 30cm before AprilTag number 0, we only need to add one further “if” statement to the code:

if (ss.getTypeID() == 0) { //if AprilTag # is 0
if (dist < 30) { //stop at 30
analogWrite(5, 0); //stop
}}

In the above example, if the AprilTag identification number is equal to 0, the self-driving car will set the motor speed to 0 when the distance from the tag is less than 30cm, via pin 5.

If we want the self-driving car to make a right turn before AprilTag number 1, we need to use an “if” statement and the variable “turn_bool”. “turn_bool” allows the self-driving car to make a right turn without repeating this execution every single time it recognizes AprilTag number 1. Elaborating on this, the VIA Pixetto will likely detect the AprilTag over and over again in the process of making the turn, hence it will execute the turn over and over again. The “turn_bool” variable acts as a filter for this, when it is set to true, the VIA Pixetto will not undergo any further turns.

For example, the initial value of “turn_bool” is false. First, when the self-driving car recognizes the AprilTag identification number as 1, it checks that distance is less than 60 cm and confirms that “turn_bool” is false
(the self-driving car is not currently performing a right turn action). If both these conditions are met, it sets “turn_bool” to true (as we are now performing a right turn action). When the self-driving car turns back to go straight, it sets “turn_bool” back to false.

else if (ss.getTypeID() == 1) { //if AprilTag # 1
if (dist < 60 && turn_bool == false) { //start turning at 60 & check turn_bool
turn_bool = true;
servo.write(125); //Turn steering servo motor to 125 degrees
delay(2600);
servo.write(90); //Turn steering motor to 90 degrees
delay(1600);
}
}

The above turning distance, angle, and duration will need to be adjusted according to each self-driving car, due to small differences with motors or the steering. This may require some experimentation!

The procedure for the self-driving car to make a left turn before it recognizes AprilTag identification number 2 is the same as the previous step when it turns right. You only need to change the number confirmed in the “if” conditional expression to 2 and the angle 125 to 35 degrees.

else if (ss.getTypeID() == 2) { //if AprilTag # is 2
if (dist < 60 && turn_bool == false) { //start turning at 60 & check turn_bool
turn_bool = true;
servo.write(35); //Turn steering servo motor to 35 degrees
delay(2600);
servo.write(90); //Turn steering servo motor to 90 degrees
delay(1000);
}
}

Again, the above turning distance, angle and duration will probably need to be adjusted according to each self-driving car.
Add an “else” statement below the “if” statement to let the “turn_bool” variable reset back to false and to make sure the car continues to move forward when VIA Pixetto has identified the object, but not the identification number.

else {
servo.write(90);
turn_bool = false;
}

We have now completed editing the program!

Finally, click the upload button in the upper left corner of the Arduino IDE to upload the written program to the Arduino development board on the self-driving car. After the display is saved, we can start testing the self-driving car!

Find a suitable venue, and place the AprilTags numbered 0 to 2 where you want the self-driving car to turn and stop. The self-driving car can then drive along the route you designed. Make sure to keep experimenting with the different angles and distances!

Thanks for completing this tutorial! Don’t forget to share your creation with the community using #VIAPixetto!

Finally, here is a copy of the final code. Feel free to copy and paste this for yourself.

#include <SmartSensor.h>
#include <Servo.h>
float focal_length = 43.04;
float tag_diag = 12.5*sqrt(2); //actual length times sqrt2
float diag;
float dist;
bool turn_bool = false;
SmartSensor ss(0,1);
Servo servo;
void setup() {
// put your setup code here, to run once:
ss.begin();
servo.attach(5);
servo.write(90);
analogWrite(5, 50);
}
void loop() {
// put your main code here, to run repeatedly:
if (ss.isDetected()) {
diag = sqrt(pow(ss.getWidth(), 2)+pow(ss.getHeight(), 2));
dist = (focal_length*tag_diag)/diag;
analogWrite(5, 50);
}
if (ss.getTypeID() == 0) { //if AprilTag # is 0
if (dist < 30) { //stop at 30
analogWrite(5, 0); //stop
}
} else if (ss.getTypeID() == 1) { //if AprilTag # 1
if (dist < 60 && turn_bool == false) { //start turning at 60 & check turn_bool
turn_bool = true;
servo.write(125); //Turn steering servo motor to 125 degrees
delay(2600);
servo.write(90); //Turn steering motor to 90 degrees
delay(1600);
}
} else if (ss.getTypeID() == 2) { //if AprilTag # is 2
if (dist < 60 && turn_bool == false) { //start turning at 60 & check turn_bool
turn_bool = true;
servo.write(35); //Turn steering servo motor to 35 degrees
delay(2600);
servo.write(90); //Turn steering servo motor to 90 degrees
delay(1000);
}
} else {
servo.write(90);
turn_bool = false;
}
}

Share this blog post!

Share on linkedin
Share on twitter
Share on facebook

Leave a Reply