Fine tuning the alignment
So far, we have the robot aligning itself fairly closely to the target, but not quite close enough. After we have finished our first turn, we need to wait a short time to let the robot stop moving and then check the target again. If it is still not close enough, we need to try and nudge the robot slightly to move it closer. One of the problems with trying to rotate the robot into position is that it takes a fair amount of power to overcome the static friction, and once that has happened, the power level is too much for the remaining dynamic friction and the robot turns too far before we can stop it.
What we are going to do is initiate a turn at a fairly low speed, and then as soon as we see the robot move at all (according to the wheel encoders), we are going to cut the power so that it only turns by a very small amount.
To accomplish this we will need three new states, NudgeLeft, NudgeRight, and Pause as follows:
private enum State
{
TurnLeft,
TurnRight,
NudgeLeft,
NudgeRight,
Pause,
Done
}
Now when the first turn is complete, we need to pause for a short time to allow the robot to stop and then enter one of the Nudge states. To start the nudge state we will create the following function
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)
{
SetState(State.Done);
}
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();
}
}
}
}
First we check to see if the target is within the acceptable shooting angle, and if it is, set the state to Done. If the angle is too great, then we determine which direction we need to turn and start turning at the define k_nudgeSpeed which is defined as follows:
private static final double k_nudgeSpeed = 0.15;
In order to determine when the robot has moved, we need to set m_nudgePos to the current position. We do this by calling the following function:
private int GetNudgePosition()
{
return(Robot.m_driveSubsystem.GetLeftEncoder().get() - Robot.m_driveSubsystem.GetRightEncoder().get());
}
Here we are returning the difference between the left and right encoders. Remember that we are turning the robot, so the encoders will be changing it opposite directions.
Next we need to change the TurningLeft and TurningRight functions so that instead of setting the state to Done they call the StartNudge function:
/*
* 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();
}
}
The Pause() function is used to enter the Pause state and is defined as follows:
private void Pause()
{
Robot.m_driveSubsystem.SetSpeed(0, 0);
m_timer.reset();
SetState(State.Pause);
}
This function resets the pause timer and sets the state. The pause timer needs to be declared as follows:
private Timer m_timer = new Timer();
Now we need to add cases to the execute() function to handle the three new states:
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;
default:
SetState(State.Done); // should not happen
break;
}
}
Once again, rather than handling these states with inline code, we call functions which are defined as follows:
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);
}
}
For the two Nudge states we need to check to see if the robot has moved. We compute absolute value of the difference between the starting nudge position and the current nudge position, and if it is greater than k_nudgeAmount, then the robot has moved. At this point, we need to once again pause and then check to see if we are close enough to the target angle, or whether we need to Nudge the robot once again. The constant k_nudgeAmount is declared as follows:
private static final int k_nudgeAmount = 6; // Minimum movement for nudge
When in the Pause state, the Pausing function will check the timer to see if the elapsed time is greater than k_pauseTime, and if it is, start the nudgging process. The constant k_pauseTime is declared as:
private static final double k_pauseTime = 0.25; // Time to wait before checking target position
Note that the program will keep switching back and forth between the Nudge states and the Pause state until the robot is at the proper shooting angle.
Your AutoShootCommand.java function 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 int m_nudgePos; // Starting nudge position
private Timer m_timer = new Timer();
private enum State
{
TurnLeft,
TurnRight,
NudgeLeft,
NudgeRight,
Pause,
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)
{
SetState(State.Done);
}
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()");
StartTurn(Robot.m_piCamera.GetCameraData());
}
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);
}
}
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;
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 the program and test it out. Pressing button 3 on the joystick should now cause the robot to rotate to the correct angle for a shot.
Here we can see that we are nicely aligned for the shot.