Auto Alignment
In order to make a shot starting from any arbitrary position on the field, we are potentially going to need to do a number of things. We will need to drive until we are roughly aligned with the front of the tower, then move forward to the correct distance, and finally adjust our angle so that the shot aligns with the target. Rather than attempting to do all of these things at once, we are going to take it step by step.
The first step, we are going to assume that the robot is the correct distance and roughly aligned with the front of the tower, but needs to be rotated to the correct position to make the shot.
Let’s start by creating a new command. Let’s call it AutoShootCommand. Go ahead and create the framework for this new command and then compare you solution with the one below. Remember that since we are going to need to both drive and shoot, this command will require the three subsystem, DriveSubsystem, SpinnerSubsystem, and ShooterSubsystem. Also, connect this new command to button 3 on the joystick.
.
.
.
.
.
You AutoShootCommand.java file should now look like:
package commands;
import robot.Robot;
import robotCore.Logger;
import wpilib.Command;
/**
*
*/
public class AutoShootCommand extends Command
{
public AutoShootCommand()
{
Logger.Log("AutoShootCommand", 3, "AutoShootCommand()");
// Use requires() here to declare subsystem dependencies
requires(Robot.m_driveSubsystem);
requires(Robot.m_shooterSubsystem);
requires(Robot.m_spinnerSubsystem);
}
// Called just before this Command runs the first time
protected void initialize()
{
Logger.Log("AutoShootCommand", 2, "initialize()");
}
// Called repeatedly when this Command is scheduled to run
protected void execute()
{
Logger.Log("AutoShootCommand", -1, "execute()");
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished()
{
Logger.Log("AutoShootCommand", -1, "isFinished()");
return (false);
}
// Called once after isFinished returns true
protected void end()
{
Logger.Log("AutoShootCommand", 2, "end()");
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted()
{
Logger.Log("AutoShootCommand", 2, "interrupted()");
}
}
And your OI.java file should look like:
package robot;
import commands.AutoShootCommand;
import commands.IntakeCommand;
import commands.ShootCommand;
import commands.SpinnerSpeedTestCommand;
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_autoShoot.whenPressed(new AutoShootCommand());
}
}
Note that for this command we are using the whenPressed function, rather than the whileHeld function. This is because we want the command to continue until the shot is complete, not as long as we are holding the button.
We are going to implement this command as a simple state machine. We start with three states and will define this by creating an enum at the top of our class as follows:
private enum State
{
TurnLeft,
TurnRight,
Done
}
private State m_state = State.Done;
When the command starts, we will check the camera and then turn right or left depending on what we see. Add the following function that will be used to start this turn:
private void StartTurn(PiCameraData data)
{
if (data != null)
{
/*
* Figure out if we need to turn left or right
*/
int centerPos = data.m_x + (data.m_width / 2);
int deltaPos = centerPos - data.m_targetHorzPos;
Logger.Log("AutoShootCommand", 1, String.format("StartTurn: deltaPos = %d", deltaPos));
if (deltaPos < 0)
{
Robot.m_driveSubsystem.SetSpeed(-k_turnSpeed, k_turnSpeed); // Turn Left
SetState(State.TurnLeft);
}
else
{
Robot.m_driveSubsystem.SetSpeed(k_turnSpeed, -k_turnSpeed); // Turn Right
SetState(State.TurnRight);
}
}
}
First we use the vision data to determine the center of the target and then look to see if it left or right of the horizontal target line, starting our turn accordingly. The turn speed k_turnSpeed constant must, of course be defined:
private static final double k_turnSpeed = 0.2;
As does the SetState functon:
void SetState(State state)
{
Logger.Log("AutoShootCommand", 1, String.format("State: %s", GetStateName(state)));
m_state = state;
}
Note that in addition to setting the m_state variable, we are logging the new state. This will help us in the debugging of our state machine.
In the initalize() function we now need call this function to begin the turn:
protected void initialize()
{
Logger.Log("AutoShootCommand", 2, "initialize()");
StartTurn(Robot.m_piCamera.GetCameraData());
}
In the execute() function we need to check to see if our turn is complete:
protected void execute()
{
Logger.Log("AutoShootCommand", -1, "execute()");
PiCameraData data = Robot.m_piCamera.GetCameraData();
switch (m_state)
{
case TurnLeft:
TurningLeft(data);
break;
case TurnRight:
TurningRight(data);
break;
default:
SetState(State.Done); // should not happen
break;
}
}
We have added a switch with case statements for each of the possible state of our state machine. Note that instead of putting the logic for each state inline in the switch we are calling functions. This is a good practice which you are encouraged to follow. Declare these two functions as follows:
/*
* Determine if the left turn is complete
*/
private void TurningLeft(PiCameraData data)
{
int centerPos = data.m_x + (data.m_width / 2);
if (centerPos > data.m_targetHorzPos)
{
SetState(State.Done);
}
}
/*
* Determine if the right turn is complete
*/
private void TurningRight(PiCameraData data)
{
int centerPos = data.m_x + (data.m_width / 2);
if (centerPos < data.m_targetHorzPos)
{
SetState(State.Done);
}
}
In either case the turn in complete when the center of the target moves past the horizontal target line. When this happens, we set the state to Done and our command is complete.
The isFinished() function should return true when the turn is complete (i.e. the state is Done):
protected boolean isFinished()
{
Logger.Log("AutoShootCommand", -1, "isFinished()");
return (m_state == State.Done);
}
Finally, when the end() or interrupted() functions are called, we need to turn the motors off:
protected void end()
{
Logger.Log("AutoShootCommand", 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("AutoShootCommand", 2, "interrupted()");
Robot.m_driveSubsystem.SetSpeed(0, 0);
}
Your AutoShootCommand.java file should now look like:
package commands;
import robot.PiCamera.PiCameraData;
import robot.Robot;
import robotCore.Logger;
import wpilib.Command;
/**
*
*/
public class AutoShootCommand extends Command
{
private static final double k_turnSpeed = 0.2;
private enum State
{
TurnLeft,
TurnRight,
Done
}
private State m_state = State.Done;
public AutoShootCommand()
{
Logger.Log("AutoShootCommand", 3, "AutoShootCommand()");
// Use requires() here to declare subsystem dependencies
requires(Robot.m_driveSubsystem);
requires(Robot.m_shooterSubsystem);
requires(Robot.m_spinnerSubsystem);
}
void SetState(State state)
{
Logger.Log("AutoShootCommand", 1, "State: " + state);
m_state = state;
}
private void StartTurn(PiCameraData data)
{
if (data != null)
{
/*
* Figure out if we need to turn left or right
*/
int centerPos = data.m_x + (data.m_width / 2);
int deltaPos = centerPos - data.m_targetHorzPos;
Logger.Log("AutoShootCommand", 1, String.format("StartTurn: deltaPos = %d", deltaPos));
if (deltaPos < 0)
{
Robot.m_driveSubsystem.SetSpeed(-k_turnSpeed, k_turnSpeed); // Turn Left
SetState(State.TurnLeft);
}
else
{
Robot.m_driveSubsystem.SetSpeed(k_turnSpeed, -k_turnSpeed); // Turn Right
SetState(State.TurnRight);
}
}
}
// Called just before this Command runs the first time
protected void initialize()
{
Logger.Log("AutoShootCommand", 2, "initialize()");
StartTurn(Robot.m_piCamera.GetCameraData());
}
/*
* Determine if the left turn is complete
*/
private void TurningLeft(PiCameraData data)
{
int centerPos = data.m_x + (data.m_width / 2);
if (centerPos > data.m_targetHorzPos)
{
SetState(State.Done);
}
}
/*
* Determine if the right turn is complete
*/
private void TurningRight(PiCameraData data)
{
int centerPos = data.m_x + (data.m_width / 2);
if (centerPos < data.m_targetHorzPos)
{
SetState(State.Done);
}
}
protected void execute()
{
Logger.Log("AutoShootCommand", -1, "execute()");
PiCameraData data = Robot.m_piCamera.GetCameraData();
switch (m_state)
{
case TurnLeft:
TurningLeft(data);
break;
case TurnRight:
TurningRight(data);
break;
default:
SetState(State.Done); // should not happen
break;
}
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished()
{
Logger.Log("AutoShootCommand", -1, "isFinished()");
return (m_state == State.Done);
}
// Called once after isFinished returns true
protected void end()
{
Logger.Log("AutoShootCommand", 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("AutoShootCommand", 2, "interrupted()");
Robot.m_driveSubsystem.SetSpeed(0, 0);
}
}
Now deploy and run your program and let’s see how this works. Move your robot so that it is at the correct position and the horizontal green line matches the horizontal red line. Then rotate the robot so that it is slightly off target to either the left or the right and press button 3. The following shows the target positions before and after the auto turn:
You will notice that it has a tendency to overshoot the target line. This is because there is a bit of a delay between the time the robot sees the target move past the center and the robot stops. To counter this, we will try and stop the robot at a point before it reaches the center line. We will define a constant for this distance as follows:
private static final int k_turnLead = 20;
And we modify the TurningLeft and TurningRight functions to stop the turn when the target is within this distance of the horizontal target line:
/*
* Determine if the left turn is complete
*/
private void TurningLeft(PiCameraData data)
{
int centerPos = data.m_x + (data.m_width / 2);
if (centerPos > data.m_targetHorzPos - k_turnLead)
{
SetState(State.Done);
}
}
/*
* Determine if the right turn is complete
*/
private void TurningRight(PiCameraData data)
{
int centerPos = data.m_x + (data.m_width / 2);
if (centerPos < data.m_targetHorzPos + k_turnLead)
{
SetState(State.Done);
}
}
Now deploy and run the program and try the Auto Shoot Command again:
As you can see, it is much closer. You can play around with the k_turnLead number to try and make it better, but you will quickly find that the amount it is off varies a fair amount making it impossible to choose a k_turnLead the will work for all turns. This turn code will get us close, but we will need another function that will allow us to make the final small adjustments required.