Navigation – Calibrate Angle
Now we need to create a new test command which records the angle. Let’s call this new command CalibrateAngleCommand. Go ahead and create this new command and connect it to our test button 12 on the joystick.
We are going to need to measure the angle of the robot on the field, and for this we will use the Gyro class. Add the following variable declaration at the top of your class:
private Gyro m_gyro = new Gyro();
As was the case for CalibrateDistanceCommand we will be using the drive motors, so we need to declare that they are required:
public CalibrateAngleCommand()
{
Logger.Log("CalibrateAngleCommand", 3, "CalibrateAngleCommand()");
// Use requires() here to declare subsystem dependencies
requires(Robot.m_driveSubsystem);
}
In the initialize() function, we need to start the robot turning, reset the Gyro, and start the log file:
protected void initialize()
{
Logger.Log("CalibrateAngleCommand", 2, "initialize()");
m_gyro.Reset();
Robot.m_driveSubsystem.SetSpeed(0.15, -0.15);
Logger.SetLogFile("CalibrateAngle", "CalibrateAngle");
Logger.Log("CalibrateAngle", 3, "angle,center,y1,y2");
}
Then in the execute() function we need to log the angle, target center, and the upper left and right corners:
protected void execute()
{
PiCameraData data = Robot.m_piCamera.GetCameraData();
Logger.Log("CalibrateAngleCommand", -1, "execute()");
Logger.Log("CalibrateAngle", 3, String.format("%d,%d,%d,%d",
m_gyro.GetYaw(),
data.m_x + data.m_width/2,
data.m_y1,
data.m_y2));
}
We will stop the turn when the robot has turned through 60 degrees:
protected boolean isFinished()
{
Logger.Log("CalibrateAngleCommand", -1, "isFinished()");
return (m_gyro.GetYaw() > 45);
}
Finally we need to turn the motors off and close the log file in the end() function:
protected void end()
{
Logger.Log("CalibrateAngleCommand", 2, "end()");
Robot.m_driveSubsystem.SetSpeed(0, 0);
Logger.CloseLogFile("CalibrateAngle");
}
Your CalibrateAngleCommand.java file should now look like:
package commands;
import robot.PiCamera.PiCameraData;
import robot.Robot;
import robotCore.Gyro;
import robotCore.Logger;
import wpilib.Command;
/**
*
*/
public class CalibrateAngleCommand extends Command
{
private Gyro m_gyro = new Gyro();
public CalibrateAngleCommand()
{
Logger.Log("CalibrateAngleCommand", 3, "CalibrateAngleCommand()");
// Use requires() here to declare subsystem dependencies
requires(Robot.m_driveSubsystem);
}
// Called just before this Command runs the first time
protected void initialize()
{
Logger.Log("CalibrateAngleCommand", 2, "initialize()");
m_gyro.Reset();
Robot.m_driveSubsystem.SetSpeed(0.15, -0.15);
Logger.SetLogFile("CalibrateAngle", "CalibrateAngle");
Logger.Log("CalibrateAngle", 3, "angle,center,y1,y2");
}
// Called repeatedly when this Command is scheduled to run
protected void execute()
{
PiCameraData data = Robot.m_piCamera.GetCameraData();
Logger.Log("CalibrateAngleCommand", -1, "execute()");
Logger.Log("CalibrateAngle", 3, String.format("%d,%d,%d,%d",
m_gyro.GetYaw(),
data.m_x + data.m_width/2,
data.m_y1,
data.m_y2));
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished()
{
Logger.Log("CalibrateAngleCommand", -1, "isFinished()");
return (m_gyro.GetYaw() > 45);
}
// Called once after isFinished returns true
protected void end()
{
Logger.Log("CalibrateAngleCommand", 2, "end()");
Robot.m_driveSubsystem.SetSpeed(0, 0);
Logger.CloseLogFile("CalibrateAngle");
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted()
{
Logger.Log("CalibrateAngleCommand", 2, "interrupted()");
}
}
Now position the robot 40 inches from the tower and rotate it to the left so that the target is as far to the right as possible while still being completely on the screen:
Then deploy and run the program and press button 12 on the joystick. The robot should slowly rotate to the right. When the command is complete, pull the log file from the robot and load it into Excel. Once again, add a column (called Top) that displays the average of Y1 and Y2 and plot the angle versus Top. You should get something like:
The peak of the graph occurs when the robot is aligned with the center of the tower. We want to offset the angle so that it is centered around this point. We can do this by creating a new Angle column which is the measured angle minus the center value which is about 21.5 degrees. Create a new plot with this new angle and fit it to a quadratic curve and you should get something like:
As before this gives us an equation which relates the Top position to the angle:
top = 0.0577*angle*angle + 286.46
Note that we have dropped the linear term (i.e. -0057*angle). This is because our equation needs to by symmetric around zero degrees.
How do we use this to adjust the distance. We know that as the robot moves toward the tower, the top position decreases. This means that since the top position is getting smaller when we turn away from the tower,the robot’s distance from the tower will be too small if we don’t adjust it. To compensate, what we want to do is multiply the position computed by the formula in the previous chapter by the ratio of the zero angle top position and the current top position:
distAdjust = 268.46 / (268.46 - 0.0577*angle*angle)
So the final equation for the distance of the robot to the tower would be:
distance = (0.000271*top *top + 0.0146*top + 16.64) * (268.46 / (268.46 - 0.0577*angle*angle))
There is one more piece of information we need to glean from the rotation log file. We want to find the relationship between the angle and the center position of the target, so we plot the angle vs the center position and we get:
As you can see, this is nice and linear and we have fitted it to a line. The equation in the lower right shows us the formula to convert the center position to the angle.
angle = 34.061 - 0.1014*center
Note that is the angle that the robot has turned away from the center of the tower. Thus the angle the robot would have to turn to get back to the center would be the negative of this or:
cAngle = 0.1014*center - 34.061