# 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

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:

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:

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:

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:

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 cAngleDistance is the distance from the robot to the tower as calculated above.

From this you can see the following relationships:

Adding this to our Update function, we get:

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:

Now we need to store our computed values of xy, 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:

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:

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:

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:

And initialize it in your robotInit function:

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:

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.