Navigator Class
Now that we know the relationships between the vision target and the robot’s position, let’s put it all together and create our navigation class. Before we start, there is some trigonometry required to convert the camera and
Gyro m_gyro = new Gyro();
Then create a Init function which we can call from our main Robot class. To make this work, we are going to want to periodically get the data from the camera and compute our location. To do this, we need to create a function that will be called on a specified schedule. To do this we will use the Schedule function of the RobotBase class to schedule our function to be called every 50 ms (i.e. 20 time a second).
The Schedule function requires Runnable class as one of it’s parameters. We can create one of these implicitly like:
public void Init()
{
RobotBase.Schedule(new Runnable()
{
@Override
public void run()
{
Update();
}
}, 50);
}
This will cause the function Update (which we will need to define) to be called every 50 ms.
In our Update function we will need to get both the current camera data and the gyro angle. We can declare the variables to do this and initialize them as follows:
private void Update()
{
PiCameraData data = Robot.m_piCamera.GetCameraData();
int angle = m_gyro.GetYaw();
}
Now the camera data is of little use to us if we cannot see the entire target in our frame, so let’s create a function that will return true only if the entire target is visible:
private boolean IsTargetVisible(PiCameraData data)
{
return( (data.m_y1 > 2) &&
(data.m_y2 > 2) &&
(data.m_x > 2) &&
((data.m_x + data.m_width) < 638));
}
The target is only completely visible if we can see the left, top, right, and bottom edges are some distance from the edge of the frame. This is what this IsTargetVisible function tests for.
At this point we want to compute both the distance of the robot from the tower, and the angle it would need to turn to be centered on the tower. Using the formulas we computed in the previous chapter this can be done as follows:
private void Update()
{
PiCameraData data = Robot.m_piCamera.GetCameraData();
int angle = m_gyro.GetYaw();
if (IsTargetVisible(data))
{
double top = (data.m_y1 + data.m_y2) / 2;
double center = (data.m_x + (data.m_width / 2));
double angleToCenter = 0.1014*center - 34.061;
double distance = (0.000271*top*top + 0.0146*top + 16.64) * (268.46 / (268.46 - 0.0577*angleToCenter*angleToCenter));
}
}
Now we want to compute the x and y position on the field, and for that we are going to need a little trigonometry. The following diagram shows what we know:
Here, gAngle measures the direction the robot is pointed given by the Yaw from the gyro, cAngle is the angle the robot must turn to align with the center of the tower as calculated above, and angle is absolute angle to the center of the tower and is the sum of gAngle and cAngle. Distance is the distance from the robot to the tower as calculated above.
From this you can see the following relationships:
angle = gAngle + cAngle; x = -Distance * sin(angle); y = Distance * cos(angle);
Adding this to our Update function, we get:
private void Update()
{
PiCameraData data = Robot.m_piCamera.GetCameraData();
int yaw = m_gyro.GetYaw();
if (IsTargetVisible(data))
{
double top = (data.m_y1 + data.m_y2) / 2;
double center = (data.m_x + (data.m_width / 2));
double angleToCenter = 0.1014*center - 34.061;
double distance = (0.000271*top*top + 0.0146*top + 16.64) * (268.46 / (268.46 - 0.0577*angleToCenter*angleToCenter));
double angle = yaw + angleToCenter;
double x = -distance * Math.sin(angle * Math.PI/180);
double y = distance * Math.cos(angle * Math.PI/180);
}
}
We want to squirrel away the x, y, and yaw so that we can access the current position and orientation of the robot from other parts of our program so we create a class and member variable which will hold these values:
private RobotPosition m_position = null;
public class RobotPosition
{
public double m_x;
public double m_y;
public double m_yaw;
RobotPosition(double x, double y, double yaw)
{
m_x = x;
m_y = y;
m_yaw = yaw;
}
}
Now we need to store our computed values of x, y, and yaw into our m_position member variable. This is where it gets a little tricky, however.
We haven’t talked about how the Schedule function works. In Java, it is possible to have different pieces of code running simultaneously. These simultaneous code segments are called threads. In this case, because it is a scheduled task, our Update function will be running in a different thread from our main program.
Now the trouble with threads is that if two different threads need to access the same data, care must be taken to ensure that both threads do not access the data at the same time. There are a number of ways to prevent this, but one of the most straightforward is to use the synchronized function:
private void Update()
{
PiCameraData data = Robot.m_piCamera.GetCameraData();
int yaw = m_gyro.GetYaw();
if ((data != null) && IsTargetVisible(data))
{
double top = (data.m_y1 + data.m_y2) / 2;
double center = (data.m_x + (data.m_width / 2));
double angleToCenter = 0.1014*center - 34.061;
double distance = (0.000271*top*top + 0.0146*top + 16.64) * (268.46 / (268.46 - 0.0577*angleToCenter*angleToCenter));
double angle = yaw + angleToCenter;
double x = -distance * Math.sin(angle * Math.PI/180);
double y = distance * Math.cos(angle * Math.PI/180);
synchronized(this)
{
m_position = new RobotPosition(x, y, yaw);
}
}
}
The syncronized function takes any Java object as a lock variable. When it is called, it first checks to see if any other thread currently owns that lock, and if not, it acquires the lock and then executes the code block that follows. If another thread currently owns the lock, this thread will halt until the lock is released by the other thread. In this particular example, this ensures that only one thread at a time will be able to access the member variable m_position.
We now need a function to give users of this class access to the current position:
public RobotPosition GetPosition()
{
synchronized(this)
{
return(m_position);
}
}
Note that we are once again using the synchronized function to prevent the Update function from changing this value while it is being accessed here by a different thread.
Your Navigator.java file should now look like:
package robot;
import robot.PiCamera.PiCameraData;
import robotCore.Gyro;
import robotCore.Logger;
import robotCore.RobotBase;
public class Navigator
{
private Gyro m_gyro = new Gyro();
private RobotPosition m_position = null;
public class RobotPosition
{
public double m_x;
public double m_y;
public double m_yaw;
RobotPosition(double x, double y, double yaw)
{
m_x = x;
m_y = y;
m_yaw = yaw;
}
}
public RobotPosition GetPosition()
{
synchronized(this)
{
return(m_position);
}
}
private boolean IsTargetVisible(PiCameraData data)
{
return( (data.m_y1 > 2) &&
(data.m_y2 > 2) &&
(data.m_x > 2) &&
((data.m_x + data.m_width) < 638));
}
private void Update()
{
PiCameraData data = Robot.m_piCamera.GetCameraData();
int yaw = m_gyro.GetYaw();
if ((data != null) && IsTargetVisible(data))
{
double top = (data.m_y1 + data.m_y2) / 2;
double center = (data.m_x + (data.m_width / 2));
double angleToCenter = 0.1014*center - 34.061;
double distance = (0.000271*top*top + 0.0146*top + 16.64) * (268.46 / (268.46 - 0.0577*angleToCenter*angleToCenter));
double angle = yaw + angleToCenter;
double x = -distance * Math.sin(angle * Math.PI/180);
double y = distance * Math.cos(angle * Math.PI/180);
synchronized(this)
{
m_position = new RobotPosition(x, y, yaw);
}
}
}
public void Init()
{
m_gyro.Reset();
RobotBase.Schedule(new Runnable()
{
@Override
public void run()
{
Update();
}
}, 50);
}
}
Now lets hook this into our Robot class and log the current position and angle when in teleop. We can then drive the robot around and see how it performs.
In your Robot class, declare an instance of your new Navigator class:
public static final Navigator m_navigator = new Navigator();
And initialize it in your robotInit function:
m_navigator.Init();
Finally in your teleopPeriodic function log the current position and angle. Also, so we don’t output too much data, increase the Sleep time to 50 ms. This delay time is temporary only, and we will change it back later. Your Robot.java file should now look like:
package robot;
import robotCore.IterativeRobot;
import robotCore.Logger;
import subsystem.DriveSubsystem;
import subsystem.ExampleSubsystem;
import subsystem.IntakeSubsystem;
import subsystem.ShooterSubsystem;
import subsystem.SpinnerSubsystem;
import wpilib.Scheduler;
public class Robot extends IterativeRobot
{
public static final String k_cameraIp = "172.24.1.1";
public static final int k_cameraPort = 5800;
public static final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem();
public static final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem();
public static final SpinnerSubsystem m_spinnerSubsystem = new SpinnerSubsystem();
public static final DriveSubsystem m_driveSubsystem = new DriveSubsystem();
public static final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
public static OI m_OI = null;
public static final PiCamera m_piCamera = new PiCamera();
public static final Navigator m_navigator = new Navigator();
Robot()
{
Logger.Log("Robot", 2, "Robot()");
}
/**
* Called once to initialize the robot
*/
@Override
public void robotInit()
{
Logger.Log("Robot", 2, "robotInit()");
m_driveSubsystem.Init();
m_exampleSubsystem.Init();
m_spinnerSubsystem.Init();
m_shooterSubsystem.Init();
m_intakeSubsystem.Init();
m_OI = new OI();
m_piCamera.Connect(k_cameraIp, k_cameraPort);
m_navigator.Init();
}
/*
* Called at the start of autonomous mode
*/
@Override
public void autonomousInit()
{
Logger.ResetElapsedTime();
Logger.Log("Robot", 2, "autonomousInit()");
}
/**
* Called periodically during autonomous
*/
@Override
public void autonomousPeriodic()
{
Logger.Log("Robot", -1, "autonomousPeriodic()");
Scheduler.getInstance().run();
Sleep(10);
}
/**
* Called at the start of teleop mode
*/
@Override
public void teleopInit()
{
Logger.ResetElapsedTime();
Logger.Log("Robot", 2, "teleopInit()");
}
/**
* Called periodically during operator control
*/
@Override
public void teleopPeriodic()
{
Logger.Log("Robot", -1, "teleopPeriodic()");
Scheduler.getInstance().run();
Navigator.RobotPosition position = m_navigator.GetPosition();
Logger.Log("Robot", 0, String.format("x = %.1f, y=$.1f, a=%.1f", position.m_x, position.m_y, position.m_yaw));
Sleep(50);
}
/**
* Called a the start of test mode
*/
@Override
public void testInit()
{
Logger.ResetElapsedTime();
Logger.Log("Robot", 2, "testInit()");
}
/**
* Called periodically during test mode
*/
@Override
public void testPeriodic()
{
Logger.Log("Robot", 0, "testPeriodic()");
Sleep(10);
}
/**
* Main program entry point
*
*/
public static void main(String args[])
{
Robot Robot = new Robot();
Robot.Start(args);
}
}
// END SNIPPET: serial-snippet
Now run deploy and run your program and observe the position information and see how closely it matches the position of the robot. Remember when you set the robot up, it must be positioned exactly facing the tower so that when we reset the gyro to zero at the start, the zero angle will be toward the tower.
You should see that it tracks the position of the robot fairly well as long as it can see the target. Once the target is no longer fully visible, all tracking information is lost.
It would be much more useful if we could find a way to track the robot’s position even when it cannot see the tower. This is what we ill tackle in the next chapter.