Drive to position
Now that we have a Navigator class which we can consult to give us the robot’s position and orientation, we want to create a way to drive to a specific point on the field. We will use this in two ways. First it will enable us to create a command which will move the robot into position to make a shot from anywhere on the field. Secondly, we will be able to use the Robot Mapper program on the driver station to control the robot.
We are going to accomplish this by creating two new commands. The first command will rotate the robot to a specific location on the field. The second command will drive to that location. Let’s call the first command RotateToPointCommand.java. Go ahead and create the framework for this new command and connect it to our test button 12 on the joystick.
In the constructor, we will need to pass in the destination point as follows:
public RotateToPointCommand(double targetX, double targetY)
{
Logger.Log("RotateToPointCommand", 3, "RotateToPointCommand()");
m_targetX = targetX;
m_targetY = targetY;
// Use requires() here to declare subsystem dependencies
requires(Robot.m_driveSubsystem);
}
Note that this command will also require the DriveSubsystem so we ask for that as well. The member variables m_targetX and m_targetY will be used to store the target location and must be delcared:
private double m_targetX; private double m_targetY;
Note that since your constructor requires an X and Y value specified, when you create the instance of this class for your OI class, you will need to pick a location. Let’s use (0, 0) which should point the robot to the base of the tower. This line in your OI.java file should look like:
m_test.whenPressed(new RotateToPointCommand(0, 0));
When the initialize() function is called, we will need to compute the angle to which we must turn to point at the target location. Using trigonometry and the robot’s current position, we can calculate this angle as follows.
protected void initialize()
{
RobotPosition position = Robot.m_navigator.GetPosition();
double dx = m_targetX - position.m_x;
double dy = m_targetY - position.m_y;
m_targetAngle = Math.atan2(dx, dy);
Logger.Log("RotateToPointCommand", 2, String.format("initialize: targetAngle = %.1f", m_targetAngle));
m_turnLeft = GetDeltaAngle(position) > 0;
}
Declaring the member variables m_targetAngle and m_turnLeft as follows:
private double m_targetAngle; private boolean m_turnLeft;
Note that we also determine whether the robot will be turning left or right. We do this by computing the difference between the current yaw and the targetAngle. If this value is greater than zero, then we will need to turn left, otherwise we return right. Since we will need to compute this deltaAngle in multiple places, we will create a function, GetDeltaAngle which we will use. This will allow us to keep this calculation in one place in case we need to modify it later.
private double GetDeltaAngle(RobotPosition position)
{
return(Navigator.NormalizeAngle(position.m_yaw - m_targetAngle));
}
Here we compute the difference between the current robot angle (yaw) and the target angle. To make things easier, we want to force this angle to be between -180 and 180 degrees and we have created a function, NormalizeAngle, which does just that. Rather than implementing this function in this command we are going to add it as a utility function to our Navigation class thus enabling us to use it for other command that we may create.
public static double NormalizeAngle(double angle)
{
if (angle > 180)
{
angle -= 360;
}
else if (angle <= -180)
{
angle += 360;
}
return(angle);
}
We make this function static so we can access it even if we don’t have an instance of the Navigation class handy.
Now back in our RotateToPointCommand class we need to change the execute() function to set the power to the motors to make the robot turn. We will want to adjust the power depending on how var from the target angle the robot is:
protected void execute()
{
Logger.Log("RotateToPointCommand", -1, "execute()");
double deltaAngle = GetDeltaAngle();
double speed = Math.abs(deltaAngle) * k_speedScale;
if (speed < k_minSpeed)
{
speed = k_minSpeed;
}
else if (speed > k_maxSpeed)
{
speed = k_maxSpeed;
}
if (m_turnLeft)
{
Robot.m_driveSubsystem.SetSpeed(-speed, speed);
}
else
{
Robot.m_driveSubsystem.SetSpeed(speed, -speed);
}
}
We set the speed based on the current deltaAngle, subject to a minimum and maximum speed and then set the motors according to which direction we are turning. The associated constants are defined as follows:
private static final double k_minSpeed = 0.10; private static final double k_maxSpeed = 0.50; private static final double k_speedScale = 0.01;
To get the deltaAngle we are once again calling GetDeltaAngle, but because we don’t have the robot’s position handy here we are creating a second version which will first obtain the position:
private double GetDeltaAngle()
{
return(GetDeltaAngle(Robot.m_navigator.GetPosition()));
}
Next we need to change isFinised() to return true when the robot has reached the correct angle:
protected boolean isFinished()
{
Logger.Log("RotateToPointCommand", -1, "isFinished()");
double deltaAngle = GetDeltaAngle();
if (m_turnLeft)
{
return(deltaAngle <= 0);
}
else
{
return(deltaAngle >= 0);
}
}
Note that the test differs depending on whether we are turning left or right.
Finally, once the turn is complete, we need to turn the motors off in the end() and interrupted() functions:
protected void end()
{
Logger.Log("RotateToPointCommand", 2, "end()");
Robot.m_driveSubsystem.SetSpeed(0, 0);
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted()
{
Logger.Log("RotateToPointCommand", 2, "interrupted()");
Robot.m_driveSubsystem.SetSpeed(0, 0);
}
Your RotateToPointCommand.java file should now look like:
package commands;
import robot.Navigator;
import robot.Navigator.RobotPosition;
import robot.Robot;
import robotCore.Logger;
import wpilib.Command;
/**
*
*/
public class RotateToPointCommand extends Command
{
private static final double k_minSpeed = 0.10;
private static final double k_maxSpeed = 0.50;
private static final double k_speedScale = 0.01;
private double m_targetX;
private double m_targetY;
private double m_targetAngle;
private boolean m_turnLeft;
public RotateToPointCommand(double targetX, double targetY)
{
Logger.Log("RotateToPointCommand", 3, "RotateToPointCommand()");
m_targetX = targetX;
m_targetY = targetY;
// Use requires() here to declare subsystem dependencies
requires(Robot.m_driveSubsystem);
}
private double GetDeltaAngle(RobotPosition position)
{
return(Navigator.NormalizeAngle(position.m_yaw - m_targetAngle));
}
private double GetDeltaAngle()
{
return(GetDeltaAngle(Robot.m_navigator.GetPosition()));
}
// Called just before this Command runs the first time
protected void initialize()
{
RobotPosition position = Robot.m_navigator.GetPosition();
double dx = m_targetX - position.m_x;
double dy = m_targetY - position.m_y;
m_targetAngle = Math.atan2(dx, dy);
Logger.Log("RotateToPointCommand", 2, String.format("initialize: targetAngle = %.1f", m_targetAngle));
m_turnLeft = GetDeltaAngle(position) > 0;
}
// Called repeatedly when this Command is scheduled to run
protected void execute()
{
Logger.Log("RotateToPointCommand", -1, "execute()");
double deltaAngle = GetDeltaAngle();
double speed = Math.abs(deltaAngle) * k_speedScale;
if (speed < k_minSpeed)
{
speed = k_minSpeed;
}
else if (speed > k_maxSpeed)
{
speed = k_maxSpeed;
}
if (m_turnLeft)
{
Robot.m_driveSubsystem.SetSpeed(-speed, speed);
}
else
{
Robot.m_driveSubsystem.SetSpeed(speed, -speed);
}
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished()
{
Logger.Log("RotateToPointCommand", -1, "isFinished()");
double deltaAngle = GetDeltaAngle();
if (m_turnLeft)
{
return(deltaAngle <= 0);
}
else
{
return(deltaAngle >= 0);
}
}
// Called once after isFinished returns true
protected void end()
{
Logger.Log("RotateToPointCommand", 2, "end()");
Robot.m_driveSubsystem.SetSpeed(0, 0);
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted()
{
Logger.Log("RotateToPointCommand", 2, "interrupted()");
Robot.m_driveSubsystem.SetSpeed(0, 0);
}
}
Your OI.java file should look like:
package robot;
import commands.AutoShootCommand;
import commands.CalibrateAngleCommand;
import commands.CalibrateDistanceCommand;
import commands.ExampleCommand;
import commands.IntakeCommand;
import commands.RotateToPointCommand;
import commands.ShootCommand;
import commands.SpinupCommand;
import robotCore.Joystick;
import wpilib.Button;
import wpilib.JoystickButton;
/**
* This class is the glue that binds the controls on the physical operator
* interface to the commands and command groups that allow control of the robot.
*/
public class OI {
//// CREATING BUTTONS
// One type of button is a joystick button which is any button on a joystick.
// You create one by telling it which joystick it's on and which button
// number it is.
Joystick m_stick = new Joystick(0);
Button m_trigger = new JoystickButton(m_stick, 1);
Button m_spinup = new JoystickButton(m_stick, 2);
Button m_intake = new JoystickButton(m_stick, 8);
Button m_test = new JoystickButton(m_stick, 12);
Button m_autoShoot = new JoystickButton(m_stick, 3);
// There are a few additional built in buttons you can use. Additionally,
// by subclassing Button you can create custom triggers and bind those to
// commands the same as any other Button.
//// TRIGGERING COMMANDS WITH BUTTONS
// Once you have a button, it's trivial to bind it to a button in one of
// three ways:
// Start the command when the button is pressed and let it run the command
// until it is finished as determined by it's isFinished method.
public OI()
{
// m_trigger.whenPressed(new ExampleCommand());
m_spinup.whileHeld(new SpinupCommand());
m_trigger.whileHeld(new ShootCommand());
m_intake.whileHeld(new IntakeCommand());
// m_test.whileHeld(new SpinnerSpeedTestCommand());
// m_test.whileHeld(new DriveSpeedTestCommand());
// m_test.whenPressed(new CalibrateDistanceCommand());
m_test.whenPressed(new RotateToPointCommand(0, 0));
// m_test.whenPressed(new ExampleCommand());
// m_test.whenPressed(new CalibrateAngleCommand());
m_autoShoot.whenPressed(new AutoShootCommand());
}
}
and your Navigator.java file should now 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)
{
}
public static double NormalizeAngle(double angle)
{
if (angle > 180)
{
angle -= 360;
}
else if (angle <= -180)
{
angle += 360;
}
return(angle);
}
}
Now go ahead and test your program. Remember that your robot must start facing the tower to get the zero angle properly set. Then rotate the robot away from the tower and press button 12. The robot should turn to face the tower.
Next: Drive to position part 2