Driver station control
We want to do one final thing before we wrap up. It would be really cool if we could control the robot by clicking on the field map on the driver station. As it turns out, the Robot Mapper program is capable of sending two different commands to the robot:
p x y g
The first command consists of the letter ‘p’ followed by and X and Y position (in inches). This command will be sent to the robot whenever you click on the map. We will use this position and the RotateToPosition and DriveToPosition commands to make the robot move to that location.
The second command consists of only the letter ‘g’ which will be sent when the Reset button on the mapper program is pressed. We will use this command to reset the gyro. The gyro we are using is fairly stable, but it does have some drift which causes it to become increasingly off over time. We can re-register the gyro position by aligning the robot so that it is exactly facing the tower, and pressing the Reset button.
Now in our Navigation class we already have a function ProcessData that will be called when the class receives data from the driver station. All we need to do is add a couple of cases to handle the data:
@Override
public void ProcessData(String command)
{
Logger.Log("Navigator", 0, String.format("ProcessData: %s", command));
switch (command.charAt(0))
{
case 'd':
Drive(command.substring(1).trim());
break;
case 'g':
ResetGyro();
break;
}
}
The Drive function will handle the position command. It will need to parse the X and Y positions and then create and call the proper RotateToPointCommand and DriveToPointCommand commands.
private void Drive(String args)
{
double a[] = ParseDoubles(args, 2);
if (a != null)
{
CommandGroup group = new CommandGroup("DriveToPoint");
group.addSequential(new RotateToPointCommand(a[0], a[1]));
group.addSequential(new DriveToPointCommand(a[0], a[1]));
group.start();
}
}
Note that we are again creating a CommandGroup and adding the two needed commands to rotate and then drive. We then can start this command immediately by calling the start() function of the CommandGroup.
Also, because the X and Y values of this command are floating point numbers, we cannot use the ParseIntegers function we used before, so we must create a new ParseDoubles function along the same line:
private double[] ParseDoubles(String str, int count)
{
double[] args = new double[count];
int i = 0;
String[] tokens = str.trim().split(" ");
for (String token : tokens)
{
try
{
args[i] = Double.parseDouble(token);
}
catch (NumberFormatException nfe)
{
break;
}
if (++i >= count)
{
break;
}
}
if (i == count)
{
return(args);
}
return(null);
}
Finally, for the reset gyro command we create:
private void ResetGyro()
{
m_gyro.Reset();
}
Your Navigator.java file should now look like:
package robot;
import commands.DriveToPointCommand;
import commands.RotateToPointCommand;
import robot.PiCamera.PiCameraData;
import robotCore.Encoder;
import robotCore.Gyro;
import robotCore.Logger;
import robotCore.Network;
import robotCore.Network.NetworkReceiver;
import wpilib.CommandGroup;
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);
}
private double[] ParseDoubles(String str, int count)
{
double[] args = new double[count];
int i = 0;
String[] tokens = str.trim().split(" ");
for (String token : tokens)
{
try
{
args[i] = Double.parseDouble(token);
}
catch (NumberFormatException nfe)
{
break;
}
if (++i >= count)
{
break;
}
}
if (i == count)
{
return(args);
}
return(null);
}
private void Drive(String args)
{
double a[] = ParseDoubles(args, 2);
if (a != null)
{
CommandGroup group = new CommandGroup("DriveToPoint");
group.addSequential(new RotateToPointCommand(a[0], a[1]));
group.addSequential(new DriveToPointCommand(a[0], a[1]));
group.start();
}
}
private void ResetGyro()
{
m_gyro.Reset();
}
@Override
public void ProcessData(String command)
{
Logger.Log("Navigator", 0, String.format("ProcessData: %s", command));
switch (command.charAt(0))
{
case 'd':
Drive(command.substring(1).trim());
break;
case 'g':
ResetGyro();
break;
}
}
public static double NormalizeAngle(double angle)
{
if (angle > 180)
{
angle -= 360;
}
else if (angle <= -180)
{
angle += 360;
}
return(angle);
}
}
Now deploy and run your program and verify that you can click on the map to make your robot move to that point. Also, line the robot up with the tower and press the Reset button on the mapper program and see that the gyro value is reset.
This concludes our tutorial.