Drive to position part 2
Now that we have the robot pointing in the correct direction, we need to create the command which will make it drive to the target point. Let’s call this new command DriveToPointCommand.
Like for the RotateToPointCommand we need to change the constructor to pass in the target X and Y position. We also need to require the DriveSubsystem.
private double m_targetX;
private double m_targetY;
public DriveToPointCommand(double targetX, double targetY)
{
Logger.Log("DriveToPointCommand", 3, "DriveToPointCommand()");
m_targetX = targetX;
m_targetY = targetY;
// Use requires() here to declare subsystem dependencies
requires(Robot.m_driveSubsystem);
}
In the execute() function, we want to use the current position and the target position to determine what angle we need to be driving at. Then based on the deviation from that angle, we want to adjust the left and right motors to make course corrections. We also want to control the speed based on how far away from the target we are, moving slower as we approach the destination.
We are also going to need to determine when we have reached the target. Just looking at the distance is likely to cause us problem. Unless we are headed exactly at the destination, we can easily slip by. So we are going to use a combination of the distance and the delta angle. If the distance is within an acceptable range, we will consider ourselves done. Alternately, if the delta angle becomes too great (indicating that we are passing the target), we will also stop.
Taking these things into account, your execute() function should look like:
protected void execute()
{
RobotPosition position = Robot.m_navigator.GetPosition();
double dx = m_targetX - position.m_x;
double dy = m_targetY - position.m_y;
double targetAngle = Math.atan2(dx, -dy) * 180 / Math.PI;
double deltaAngle = Navigator.NormalizeAngle(position.m_yaw - targetAngle);
double dist = Math.sqrt(dx*dx + dy*dy);
Logger.Log("DriveToPointCommand", -1, "execute()");
if ((dist < k_minDistance) || (Math.abs(deltaAngle) > k_maxAngle))
{
m_done = true;
}
else
{
double speed = dist * k_speedScale;
double adjust = speed * deltaAngle * k_angleScale;
if (speed > k_maxSpeed)
{
speed = k_maxSpeed;
}
else if (speed < k_minSpeed)
{
speed = k_minSpeed;
}
Logger.Log("DriveToPointCommane", 0, String.format("speed=%.1f,adjust=%.2f", speed, adjust));
Robot.m_driveSubsystem.SetSpeed(speed - adjust, speed + adjust);
}
}
We will use the member variable m_done to indicate whether we are at the target or not. Here we see that we are going to set it to true if either the distance to the target is less than k_minDistance, or the delta angle is greater than k_maxAngle.
We must define m_done and the various constants as follows:
private static final double k_maxAngle = 30; private static final double k_minDistance = 2; private static final double k_angleScale = 0.01; private static final double k_speedScale = 0.05; private static final double k_minSpeed = 0.20; private static final double k_maxSpeed = 0.60; private boolean m_done;
We will also want to set m_done to false in our initialize() function:
protected void initialize()
{
Logger.Log("DriveToPointCommand", 2, "initialize()");
m_done = false;
}
In our isFinished() function we need to return true when the robot has reached the target.
protected boolean isFinished()
{
Logger.Log("DriveToPointCommand", -1, "isFinished()");
return (m_done);
}
Finally, in our end() and interrupted() functions we need to stop the motors:
protected void end()
{
Logger.Log("DriveToPointCommand", 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("DriveToPointCommand", 2, "interrupted()");
Robot.m_driveSubsystem.SetSpeed(0, 0);
}
Your DriveToPointCommand.java file should now look like:
package commands;
import robot.Navigator;
import robot.Robot;
import robot.Navigator.RobotPosition;
import robotCore.Logger;
import wpilib.Command;
/**
*
*/
public class DriveToPointCommand extends Command
{
private static final double k_maxAngle = 30;
private static final double k_minDistance = 2;
private static final double k_angleScale = 0.01;
private static final double k_speedScale = 0.05;
private static final double k_minSpeed = 0.20;
private static final double k_maxSpeed = 0.60;
private double m_targetX;
private double m_targetY;
private boolean m_done;
public DriveToPointCommand(double targetX, double targetY)
{
Logger.Log("DriveToPointCommand", 3, "DriveToPointCommand()");
m_targetX = targetX;
m_targetY = targetY;
// 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("DriveToPointCommand", 2, "initialize()");
m_done = false;
}
// Called repeatedly when this Command is scheduled to run
protected void execute()
{
RobotPosition position = Robot.m_navigator.GetPosition();
double dx = m_targetX - position.m_x;
double dy = m_targetY - position.m_y;
double targetAngle = Math.atan2(dx, -dy) * 180 / Math.PI;
double deltaAngle = Navigator.NormalizeAngle(position.m_yaw - targetAngle);
double dist = Math.sqrt(dx*dx + dy*dy);
Logger.Log("DriveToPointCommand", -1, "execute()");
if ((dist < k_minDistance) || (Math.abs(deltaAngle) > k_maxAngle))
{
m_done = true;
}
else
{
double speed = dist * k_speedScale;
double adjust = speed * deltaAngle * k_angleScale;
if (speed > k_maxSpeed)
{
speed = k_maxSpeed;
}
else if (speed < k_minSpeed)
{
speed = k_minSpeed;
}
Logger.Log("DriveToPointCommane", 0, String.format("speed=%.1f,adjust=%.2f", speed, adjust));
Robot.m_driveSubsystem.SetSpeed(speed - adjust, speed + adjust);
}
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished()
{
Logger.Log("DriveToPointCommand", -1, "isFinished()");
return (m_done);
}
// Called once after isFinished returns true
protected void end()
{
Logger.Log("DriveToPointCommand", 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("DriveToPointCommand", 2, "interrupted()");
Robot.m_driveSubsystem.SetSpeed(0, 0);
}
}
Now we want to connect this new command to our test button. However, this command will only work if the robot is pointed at the target location. What we really need to do when the test button is pressed is to first call our RotateToPointCommand and then this new DriveToPointCommand. Fortunately, the command processor provides a way to do this.
We can execute a group of commands by creating an instance the CommandGroup class and then add the command that we want executed by adding the following code to our OI class:
CommandGroup group = new CommandGroup("DriveToPoint");
group.addSequential(new RotateToPointCommand(0, 40));
group.addSequential(new DriveToPointCommand(0, 40));
m_test.whenPressed(group);
First we create the group, then add our commands to it, and set it so that the group is executed when our test button is pressed. Note that we have chosen an arbitrary point (0, 40) and are going to have the robot turn to face that point and then drive to it. Your OI.java file should now look like:
package robot;
import commands.AutoShootCommand;
import commands.CalibrateAngleCommand;
import commands.CalibrateDistanceCommand;
import commands.DriveToPointCommand;
import commands.ExampleCommand;
import commands.IntakeCommand;
import commands.RotateToPointCommand;
import commands.ShootCommand;
import commands.SpinupCommand;
import robotCore.Joystick;
import wpilib.Button;
import wpilib.CommandGroup;
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 DriveToPointCommand(0, 30));
// m_test.whenPressed(new ExampleCommand());
// m_test.whenPressed(new CalibrateAngleCommand());
m_autoShoot.whenPressed(new AutoShootCommand());
CommandGroup group = new CommandGroup("DriveToPoint");
group.addSequential(new RotateToPointCommand(0, 40));
group.addSequential(new DriveToPointCommand(0, 40));
m_test.whenPressed(group);
}
}
Now deploy and run your program. Remember that when you start the robot, it must be pointed directly at the tower so that the gyro zero point is set correctly. Then you should be able to move the robot to any point on the field, and when you press the test button (12), the robot should turn and drive to a point 40 inches directly in front of the tower.
Before we move on, let’s create a group command which will cause the robot to drive and shoot from anywhere on the field. Let’s connect this command to button 4 on the joystick:
group = new CommandGroup("DriveAndShoot");
group.addSequential(new RotateToPointCommand(0, 40));
group.addSequential(new DriveToPointCommand(0, 40));
group.addSequential(new RotateToPointCommand(0, 0));
group.addSequential(new AutoShootCommand());
m_driveAndShoot.whenPressed(group);
Here we create a new group and add commands that will make the robot drive to position (0, 40) in front of the tower (as before), then turn to face the tower, and finally run the AutoShootCommand to make the shot. We, of course, need to define this new button:
Button m_driveAndShoot = new JoystickButton(m_stick, 4);
At this point your OI.java file should look like:
package robot;
import commands.AutoShootCommand;
import commands.CalibrateAngleCommand;
import commands.CalibrateDistanceCommand;
import commands.DriveToPointCommand;
import commands.ExampleCommand;
import commands.IntakeCommand;
import commands.RotateToPointCommand;
import commands.ShootCommand;
import commands.SpinupCommand;
import robotCore.Joystick;
import wpilib.Button;
import wpilib.CommandGroup;
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);
Button m_driveAndShoot = new JoystickButton(m_stick, 4);
// 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 DriveToPointCommand(0, 30));
// m_test.whenPressed(new ExampleCommand());
// m_test.whenPressed(new CalibrateAngleCommand());
m_autoShoot.whenPressed(new AutoShootCommand());
CommandGroup group = new CommandGroup("DriveToPoint");
group.addSequential(new RotateToPointCommand(0, 40));
group.addSequential(new DriveToPointCommand(0, 40));
m_test.whenPressed(group);
group = new CommandGroup("DriveAndShoot");
group.addSequential(new RotateToPointCommand(0, 40));
group.addSequential(new DriveToPointCommand(0, 40));
group.addSequential(new RotateToPointCommand(0, 0));
group.addSequential(new AutoShootCommand());
m_driveAndShoot.whenPressed(group);
}
}
Now deploy and run your program and you should find that you can place the robot anywhere on the field, and it will line up with the tower and make the shot.