Making the shot
Now that we have the robot positioned at the right angle, we are ready to make the shot. Of course, in order to shoot the ball we will need to spin up the shooter motors. Since it takes time for the motors to reach the proper speed, we don’t want to wait until we have aligned with the target to start them, so we will turn them on before we start the alignment process by adding the following line to the initialize() function:
Robot.m_spinnerSubsystem.SpinUp();
When the Nudge process is complete, we don’t want to stop, but rather prepare for the shot which means waiting for the spinner motors to reach full speed. To accomplish this goal, we will need a function in the SpinnerSubsystem that we can call that will tell us that the motors have reached the designated speed. Add the following function to your SpinnerSubsystem class:
public boolean IsAtSpeed(int tolerance)
{
return(m_leftController.IsAtSpeed(tolerance) && m_rightController.IsAtSpeed(tolerance));
}
This function will return true if both spinner motors are withing a specified tolerance of their set speed. Your SpinnerSubsystem.java file should now look like
package subsystem;
import robotCore.I2CPWMMotor;
import robotCore.IREncoder;
import robotCore.Logger;
import robotCore.MotorSpeedController;
import wpilib.Subsystem;
/**
*
*/
public class SpinnerSubsystem extends Subsystem
{
private static final double k_intakeSpeed = -0.8;
private static final int k_rightSpinnerPin1 = 6;
private static final int k_rightSpinnerPin2 = 7;
private static final int k_leftSpinnerPin1 = 10;
private static final int k_leftSpinnerPin2 = 11;
private static final int k_leftEncoderPort = 1;
private static final int k_rightEncoderPort = 0;
private I2CPWMMotor m_rightSpinner = null;
private I2CPWMMotor m_leftSpinner = null;
private IREncoder m_leftEncoder = null;
private IREncoder m_rightEncoder = null;
private MotorSpeedController m_leftController = null;
private MotorSpeedController m_rightController = null;
public void initDefaultCommand()
{
Logger.Log("SpinnerSubsystem", 2, "initDefaultCommand()");
}
public void Init()
{
m_rightSpinner = new I2CPWMMotor(k_rightSpinnerPin1, k_rightSpinnerPin2);
m_leftSpinner = new I2CPWMMotor(k_leftSpinnerPin1, k_leftSpinnerPin2);
m_leftEncoder = new IREncoder(k_leftEncoderPort);
m_rightEncoder = new IREncoder(k_rightEncoderPort);
m_leftController = new MotorSpeedController(m_leftSpinner, m_leftEncoder, 0.02, 0.0025, 0, 0.01);
m_rightController = new MotorSpeedController(m_rightSpinner, m_rightEncoder, 0.02, 0.0025, 0, 0.01);
m_leftController.SetIRange(10);
m_rightController.SetIRange(10);
Logger.Log("SpinnerSubsystem", 2, "Init()");
}
public void SpinUp()
{
m_rightController.set(75);
m_leftController.set(75);
}
public void Stop()
{
m_rightController.set(0);
m_leftController.set(0);
}
public void Intake()
{
m_rightSpinner.set(k_intakeSpeed);
m_leftSpinner.set(k_intakeSpeed);
}
public IREncoder GetLeftEncoder()
{
return(m_leftEncoder);
}
public IREncoder GetRightEncoder()
{
return(m_rightEncoder);
}
public boolean IsAtSpeed(int tolerance)
{
return(m_leftController.IsAtSpeed(tolerance) && m_rightController.IsAtSpeed(tolerance));
}
}
Now back in our AutoShootCommand class, add two new states, PrepareShot, and Shoot:
private enum State
{
TurnLeft,
TurnRight,
NudgeLeft,
NudgeRight,
Pause,
PrepareShot,
Shoot,
Done
}
In the StartNudge function we currently check to see if the robot is aligned correctly and then enter the Done state if it is. We want to change this so that we stop the turning and enter the PrepareShot state instead:
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();
}
}
}
}
Now add a new case in the execute() function to handle the PrepareShot case:
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;
default:
SetState(State.Done); // should not happen
break;
}
}
Once again we call a function PreparingShot to handle this state which we declare as follows:
private void PreparingShot(PiCameraData data)
{
if (Robot.m_spinnerSubsystem.IsAtSpeed(5))
{
Shoot();
}
}
Here we wait until the speed of the motors are withing 5 RPS of the target and then call the function Shoot() which will activate the shooting mechanism and place us in the Shoot state. The Shoot() function is declared as follows:
private void Shoot()
{
Robot.m_shooterSubsystem.Shoot();
m_timer.reset();
SetState(State.Shoot);
}
We activate the shooter mechanism, start a timer, and set the state to Shoot. We use a timer here because we need to leave the shooter mechanism active for a short time to allow the ball to be grabbed by the spinning wheels. When the timer expires, we will then release the shooting mechanism. We check for this condition by adding the last Shoot case to our execute() function as follows:
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;
default:
SetState(State.Done); // should not happen
break;
}
}
And we declare the Shooting() function as follows
protected void Shooting()
{
if (m_timer.get() > k_shootTime)
{
SetState(State.Done);
}
}
Once the elapsed time exceeds k_shootTime we enter the Done state. The k_shootTime constant is defined as:
private static final double k_shootTime = 0.30; // Time to leave the shooter engaged
We need to do one last thing. When the end() or interrupted() functions are called, we need to ensure that the spinner motors are turned off and the shooter mechanism is in the release position as follows:
// 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();
}
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 int m_nudgePos; // Starting nudge position
private Timer m_timer = new Timer();
private enum State
{
TurnLeft,
TurnRight,
NudgeLeft,
NudgeRight,
Pause,
PrepareShot,
Shoot,
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());
}
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);
}
}
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;
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();
}
}
Now deploy and run your program and test it out. Position your robot so that it is at the correct shooting distance, but rotate it so that it is not aligned with the target. Then press button 3 on the joystick and the robot should turn to the correct position and shoot the ball.