Plotting the position
Printing out the coordinates is fine for testing, but what we would really like to do is to display the position of the robot on a map of the field displayed on the driver station. For this, we will need a program running on the driver station to handle the plotting of the map. Fortunately, we have one called RobotMapper. This program connects to the robot, and accepts commands which are used to display the current robot position. It currently will accept the following two commands (as ascii strings):
c yaw xPos yPos p yaw xPos yPos
The first character determines the command. This is followed by the current yaw of the robot (in degrees), and the current X and Y position in inches. the ‘c’ command tells the mapper that the data is coming from the camera, and the ‘p’ command tells the mapper that the data is being computed using the dead reckoning.
The first thing we will need to do is add an instance of our Network class. We declare it as follows:
private Network m_network = null;
And initialize it in our Init() function:
m_network = new Network(); m_network.Listen(this, k_networkPort);
This creates a new instance of our Network class and then tells it to listen for an incoming network connection on port k_networkPort which is defined as:
private static final int k_networkPort = 5803;
This will be the port to which our RobotMapper program will connect.
After we add this code, we will see that there is an error. This is because we are passing this instance of our Navigator class as the NetworkReceiver parameter of the Listen function. In order to do this, our Navigator class must implement this interface. We do this by changing the declaration of our class as follows:
public class Navigator implements NetworkReceiver
When we do this, it then tells us that we need to add the unimplemented methods of the NetworkReceiver interface, which we do as follows:
@Override
public void ProcessData(String command) {
// TODO Auto-generated method stub
}
This function will be called by the Network class whenever it receives messages from the driver station. We will see how we might use this later.
For now, all we need to do is add code to send commands to the RobotMapper program running on the driver station to update the robot’s position and orientation. Change the Update() function to add two SendMessage calls to the Network class as follows:
private void Update()
{
PiCameraData data = Robot.m_piCamera.GetCameraData();
int yaw = m_gyro.GetYaw();
int leftEncoder = m_leftEncoder.get();
int rightEncoder = m_rightEncoder.get();
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);
}
m_network.SendMessage(String.format("c %.1f %.2f %.2f", m_position.m_yaw, m_position.m_x, m_position.m_y));
}
else if (m_position != null)
{
double deltaLeft = (leftEncoder - m_lastLeftEncoder) / k_ticksPerInch;
double deltaRight = (rightEncoder - m_lastRightEncoder) / k_ticksPerInch;
double distance = (deltaLeft + deltaRight) / 2;
if (distance != 0)
{
double radians = (m_position.m_yaw * Math.PI) / 180;
double deltaX = distance * Math.sin(radians);
double deltaY = distance * Math.cos(radians);
synchronized(this)
{
m_position = new RobotPosition(m_position.m_x + deltaX, m_position.m_y - deltaY, yaw);
}
m_network.SendMessage(String.format("p %.1f %.2f %.2f", m_position.m_yaw, m_position.m_x, m_position.m_y));
}
}
m_lastLeftEncoder = leftEncoder;
m_lastRightEncoder = rightEncoder;
Robot.Sleep(50);
}
At this point your Navigator.java file should look like:
package robot;
import robot.PiCamera.PiCameraData;
import robotCore.Encoder;
import robotCore.Gyro;
import robotCore.Logger;
import robotCore.Network;
import robotCore.Network.NetworkReceiver;
import robotCore.RobotBase;
public class Navigator implements NetworkReceiver
{
private static final int k_networkPort = 5803;
private static final double k_ticksPerInch = 8000.0/38.5;
private Gyro m_gyro = new Gyro();
private RobotPosition m_position = null;
private Encoder m_leftEncoder = null;
private Encoder m_rightEncoder = null;
private int m_lastLeftEncoder = 0;
private int m_lastRightEncoder = 0;
private Network m_network = 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();
int leftEncoder = m_leftEncoder.get();
int rightEncoder = m_rightEncoder.get();
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);
}
m_network.SendMessage(String.format("c %.1f %.2f %.2f", m_position.m_yaw, m_position.m_x, m_position.m_y));
}
else if (m_position != null)
{
double deltaLeft = (leftEncoder - m_lastLeftEncoder) / k_ticksPerInch;
double deltaRight = (rightEncoder - m_lastRightEncoder) / k_ticksPerInch;
double distance = (deltaLeft + deltaRight) / 2;
if (distance != 0)
{
double radians = (m_position.m_yaw * Math.PI) / 180;
double deltaX = distance * Math.sin(radians);
double deltaY = distance * Math.cos(radians);
synchronized(this)
{
m_position = new RobotPosition(m_position.m_x + deltaX, m_position.m_y - deltaY, yaw);
}
m_network.SendMessage(String.format("p %.1f %.2f %.2f", m_position.m_yaw, m_position.m_x, m_position.m_y));
}
}
m_lastLeftEncoder = leftEncoder;
m_lastRightEncoder = rightEncoder;
Robot.Sleep(50);
}
public void Init()
{
m_gyro.Reset();
m_leftEncoder = Robot.m_driveSubsystem.GetLeftEncoder().Clone();
m_rightEncoder = Robot.m_driveSubsystem.GetRightEncoder().Clone();
RobotBase.Schedule(new Runnable()
{
@Override
public void run()
{
Update();
}
}, 50);
m_network = new Network();
m_network.Listen(this, k_networkPort);
}
@Override
public void ProcessData(String command) {
// TODO Auto-generated method stub
}
}
Before you run your program, launch the RobotMapper program by double clicking on the RobotMapper.cmd file in the PiUtil folder. Then launch your program and drive the robot around. Remember that you need to align the robot so that it pointing directly at the tower to get the zero point of the gyro set correctly.
You should see it’s position tracked on the map. You will notice that when the robot can see the tower you see only one green marker. When the robot cannot see the tower, you see both a red and green marker. The green marker shows the robot’s position using the dead reckoning, and the red marker shows the last position where the robot could see the tower.