Getting the distance right
We now have a robot that is capable of rotating to the correct angle for the shot, but we still need to add code that will cause it do move to the correct distance. When we do this, we are going to assume that the robot is roughly aligned with the tower, but needs to be moved and rotated into the proper position.
In our AutoShootCommand class, we need to add a new state Drive which we will use to drive the robot to the correct distance from the tower. When this state ends, it will transition to the TurnLeft or TurnRight states, and from their to eventually making the shot.
private enum State
{
TurnLeft,
TurnRight,
NudgeLeft,
NudgeRight,
Pause,
PrepareShot,
Shoot,
Drive,
Done
}
Currently in the initialize() class we call the StartTurn function to start the robot turning toward the target. Instead, we now want to set the state to our new Drive state as follows:
protected void initialize()
{
Logger.Log("AutoShootCommand", 2, "initialize()");
Robot.m_spinnerSubsystem.SpinUp();
SetState(State.Drive);
}
We must now add a new case in our execute() function to handle this new state:
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;
case NudgeLeft:
case NudgeRight:
Nudging(data);
break;
case Pause:
Pausing(data);
break;
case PrepareShot:
PreparingShot(data);
break;
case Shoot:
Shooting();
break;
case Drive:
Driving(data);
break;
default:
SetState(State.Done); // should not happen
break;
}
}
The new Driving function will handle the processing for this new state and is defined as follows:
private void Driving(PiCameraData data)
{
int topPos = (data.m_y1 + data.m_y2) / 2;
int centerPos = data.m_x + (data.m_width / 2);
/*
* Drive forward while correcting angle until in range
*/
if (topPos < (data.m_targetVertPos + k_driveLead))
{
/*
* Robot in range
*/
StartTurn(data);
}
else
{
int deltaTargetPos = centerPos - data.m_targetHorzPos; // Amount robot is deviating from target
double deltaSpeed = deltaTargetPos * k_turnScale; // Adjust angle based on deviation from target
double speed = (topPos - data.m_targetVertPos) * k_speedScale; // Adjust speed based on distance from target
if (speed < k_minSpeed)
{
speed = k_minSpeed;
}
else if (speed > k_maxSpeed)
{
speed = k_maxSpeed;
}
Robot.m_driveSubsystem.SetSpeed(speed + deltaSpeed, speed - deltaSpeed);
}
}
Let’s take a look at this function in detail. The first thing we do is compute the top position of our target (topPos). Note that we are not using the top of the bounding box for this purpose, but rather the average of the upper left and upper right corners. We then compare this to the current position of the horizontal target line. Now we could stop when the topPos is less than the horizontal target line (data.m_targetHorzPos) but as we discovered with the turning, this will cause our robot to overshoot the target by a small amount. Thus we add k_driveLead to the target line and stop when that is reached. The robot will then coast a little farther. The constant k_driveLead is defined as follows:
private static final int k_driveLead = 25; // Lead distance for drive
If we have not yet reached the position that we need, then we need to set the speed of the motors. We want to adjust the speed based on how far we currently are from the target. The following line computes this speed by taking the difference between our topPos and the horizontal target line. This number is multiplied by the scale factor k_speedScale to give us a power value:
double speed = (topPos - data.m_targetVertPos) * k_speedScale; // Adjust speed based on distance from target
The constant k_speedScale is defined as follows:
private static final double k_speedScale = 0.01; // Scale factor to adjust speed based on distance
After we compute this power we make sure that the value is between some minimum (k_minSpeed) and some maximum (k_maxSpeed) speed to make sure we don’t go either too fast or too slow. These constants are defined as:
private static final double k_minSpeed = 0.20; // Minimum driving speed private static final double k_maxSpeed = 0.50; // Maximum driving speed
Finally, as long as we are driving, we might as well try and adjust the angle at the same time. This is what the following two lines try and do:
int deltaTargetPos = centerPos - data.m_targetHorzPos; // Amount robot is deviating from target double deltaSpeed = deltaTargetPos * k_turnScale; // Adjust angle based on deviation from target
The first line computes the difference between our target center (centerPos) and the vertical target line. We then multiply this value by the scale factor k_turnScale to obtain a number which we add to the left motor and subtract from the right motor which will make the robot turn slightly in the correct direction. The constant k_turnScale is defined as:
private static final double k_turnScale = 0.002; // Turning scale factor for adjusting angle while driving.
Your AutoShootCommand.java file should now look like:
package commands;
import robot.PiCamera.PiCameraData;
import robot.Robot;
import robotCore.Logger;
import robotCore.Timer;
import wpilib.Command;
/**
*
*/
public class AutoShootCommand extends Command
{
private static final double k_turnSpeed = 0.2;
private static final int k_turnLead = 30;
private static final double k_nudgeSpeed = 0.15;
private static final int k_minTargetPos = 10; // Minimum acceptable error for shot
private static final int k_nudgeAmount = 6; // Minimum movement for nudge
private static final double k_pauseTime = 0.25; // Time to wait before checking target position
private static final double k_shootTime = 0.30; // Time to leave the shooter engaged
private static final int k_driveLead = 25; // Lead distance for drive
private static final double k_turnScale = 0.002; // Turning scale factor for adjusting angle while driving.
private static final double k_minSpeed = 0.20; // Minimum driving speed
private static final double k_maxSpeed = 0.50; // Maximum driving speed
private static final double k_speedScale = 0.01; // Scale factor to adjust speed based on distance
private int m_nudgePos; // Starting nudge position
private Timer m_timer = new Timer();
private enum State
{
TurnLeft,
TurnRight,
NudgeLeft,
NudgeRight,
Pause,
PrepareShot,
Shoot,
Drive,
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);
}
}
}
private int GetNudgePosition()
{
return(Robot.m_driveSubsystem.GetLeftEncoder().get() - Robot.m_driveSubsystem.GetRightEncoder().get());
}
private void StartNudge(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;
if (Math.abs(deltaPos) <= k_minTargetPos)
{
Robot.m_driveSubsystem.SetSpeed(0, 0);
SetState(State.PrepareShot);
}
else
{
if (deltaPos < 0)
{
Robot.m_driveSubsystem.SetSpeed(-k_nudgeSpeed, k_nudgeSpeed); // Turn Left
SetState(State.NudgeLeft);
m_nudgePos = GetNudgePosition();
}
else
{
Robot.m_driveSubsystem.SetSpeed(k_nudgeSpeed, -k_nudgeSpeed); // Turn Right
SetState(State.NudgeLeft);
m_nudgePos = GetNudgePosition();
}
}
}
}
// Called just before this Command runs the first time
protected void initialize()
{
Logger.Log("AutoShootCommand", 2, "initialize()");
Robot.m_spinnerSubsystem.SpinUp();
// StartTurn(Robot.m_piCamera.GetCameraData());
SetState(State.Drive);
}
private void Pause()
{
Robot.m_driveSubsystem.SetSpeed(0, 0);
m_timer.reset();
SetState(State.Pause);
}
/*
* 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)
{
Pause();
}
}
/*
* 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)
{
Pause();
}
}
private void Nudging(PiCameraData data)
{
int diff = Math.abs(GetNudgePosition() - m_nudgePos);
if (diff > k_nudgeAmount)
{
Pause();
}
}
private void Pausing(PiCameraData data)
{
if (m_timer.get() > k_pauseTime)
{
StartNudge(data);
}
}
private void Shoot()
{
Robot.m_shooterSubsystem.Shoot();
m_timer.reset();
SetState(State.Shoot);
}
private void PreparingShot(PiCameraData data)
{
if (Robot.m_spinnerSubsystem.IsAtSpeed(5))
{
Shoot();
}
}
protected void Shooting()
{
if (m_timer.get() > k_shootTime)
{
SetState(State.Done);
}
}
private void Driving(PiCameraData data)
{
int topPos = (data.m_y1 + data.m_y2) / 2;
int centerPos = data.m_x + (data.m_width / 2);
/*
* Drive forward while correcting angle until in range
*/
if (topPos < (data.m_targetVertPos + k_driveLead))
{
/*
* Robot in range
*/
StartTurn(data);
}
else
{
int deltaTargetPos = centerPos - data.m_targetHorzPos; // Amount robot is deviating from target
double deltaSpeed = deltaTargetPos * k_turnScale; // Adjust angle based on deviation from target
double speed = (topPos - data.m_targetVertPos) * k_speedScale; // Adjust speed based on distance from target
if (speed < k_minSpeed)
{
speed = k_minSpeed;
}
else if (speed > k_maxSpeed)
{
speed = k_maxSpeed;
}
Robot.m_driveSubsystem.SetSpeed(speed + deltaSpeed, speed - deltaSpeed);
}
}
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;
case NudgeLeft:
case NudgeRight:
Nudging(data);
break;
case Pause:
Pausing(data);
break;
case PrepareShot:
PreparingShot(data);
break;
case Shoot:
Shooting();
break;
case Drive:
Driving(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);
Robot.m_spinnerSubsystem.Stop();
Robot.m_shooterSubsystem.ReleaseShooter();
}
// 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);
Robot.m_spinnerSubsystem.Stop();
Robot.m_shooterSubsystem.ReleaseShooter();
}
}
When you now deploy and run your program, you will find that all you need to do is position the robot so it is roughly aligned with the tower and it will drive and shoot automatically when you press button 3 on the joystick: