Intake Subsystem
Now that we have the robot driving and shooting, the last thing we need is some way to pick up the ball from the ground. The robot has two motors attached to two spinning wheels which can pick up the ball and deliver it to the shooter wheels, which, if run backwards, and deliver the ball to the plastic cup which holds it in position for shooting.
We need to create a subsystem to operate those motors. We will call this subsystem IntakeSubsystem. Create a new class of this name under the subsystem folder in your project. As before copy the contents of your ExampleSubsystem.java file and past it into your newly created IntakeSubsystem.java file. Then replace all instances of ExampleSubsystem with IntakeSubsystem. Your IntakeSubsystem.java file should now look like:
package subsystem;
import robotCore.Logger;
import wpilib.Subsystem;
/**
*
*/
public class IntakeSubsystem extends Subsystem
{
public void initDefaultCommand()
{
Logger.Log("IntakeSubsystem", 2, "initDefaultCommand()");
}
public void Init()
{
Logger.Log("IntakeSubsystem", 2, "Init()");
}
}
Create two new variables to control the two intake motors by adding the following lines to the beginning of the IntakeSubsystem class (be sure to import the I2CPWMMotor class:
private I2CPWMMotor m_rightIntake = null; private I2CPWMMotor m_leftIntake = null;
Then initialize these in the init() function with the following two lines:
m_rightIntake = new I2CPWMMotor(m_rightPin1, m_rightPin2);
m_leftIntake = new I2CPWMMotor(m_leftPin1, m_leftPin2);
As before the I2CPWMMotor constructor requires the two pin numbers to which the motor is attached, and, as before, we are using constants here instead of the actual number. Of course this means that we must define these constants, which we can do by adding the following lines to the top of the class:
private static final int m_rightPin1 = 4; private static final int m_rightPin2 = 5; private static final int m_leftPin1 = 8; private static final int m_leftPin2 = 9;
Finally we need to create functions that will turn the motors on so that they will intake a ball. We will also need a function that stops the motors once we have captured the ball. Add the following two functions to your class:
public void Intake()
{
m_rightIntake.set(1.0);
m_leftIntake.set(1.0);
}
public void Stop()
{
m_rightIntake.set(0.0);
m_leftIntake.set(0.0);
}
The Intake() function will turn the motors on full power in the direction needed to capture the ball. The Stop() function will turn the motors back off.
Your IntakeSubsystem.java file should now look like:
package subsystem;
import robotCore.I2CPWMMotor;
import robotCore.Logger;
import wpilib.Subsystem;
/**
*
*/
public class IntakeSubsystem extends Subsystem
{
private static final int m_rightPin1 = 4;
private static final int m_rightPin2 = 5;
private static final int m_leftPin1 = 8;
private static final int m_leftPin2 = 9;
private I2CPWMMotor m_rightIntake = null;
private I2CPWMMotor m_leftIntake = null;
public void initDefaultCommand()
{
Logger.Log("IntakeSubsystem", 2, "initDefaultCommand()");
}
public void Init()
{
Logger.Log("IntakeSubsystem", 2, "Init()");
m_rightIntake = new I2CPWMMotor(m_rightPin1, m_rightPin2);
m_leftIntake = new I2CPWMMotor(m_leftPin1, m_leftPin2);
}
public void Intake()
{
m_rightIntake.set(1.0);
m_leftIntake.set(1.0);
}
public void Stop()
{
m_rightIntake.set(0.0);
m_leftIntake.set(0.0);
}
}
We are now ready to add an instance of this class to our Robot class. Open Robot.java and the following line to the beginning of your Robot class (remember ti import the IntakeSubsystem class as well):
public static final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem();
Now initialize this subsystem by adding the following line to the robotInit() function:
m_intakeSubsystem.Init();
We now need to create a command which will operate the intake mechanism, which we will call the IntakeCommand. Create a new class of the same name under the commands folder, and copy the contents of ExampleCommand.java into the newly created file. Then replace all occurrences of ExampleCommand with IntakeCommand. Your IntakeCommand.java file should now look like:
package commands;
import robotCore.Logger;
import wpilib.Command;
/**
*
*/
public class ExampleCommand extends Command
{
public ExampleCommand()
{
Logger.Log("ExampleCommand", 3, "ExampleCommand()");
// Use requires() here to declare subsystem dependencies
// requires(Robot.m_exampleSubsystem);
}
// Called just before this Command runs the first time
protected void initialize()
{
Logger.Log("ExampleCommand", 2, "initialize()");
}
// Called repeatedly when this Command is scheduled to run
protected void execute()
{
Logger.Log("ExampleCommand", -1, "execute()");
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished()
{
Logger.Log("ExampleCommand", -1, "isFinished()");
return (false);
}
// Called once after isFinished returns true
protected void end()
{
Logger.Log("ExampleCommand", 2, "end()");
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted()
{
Logger.Log("ExampleCommand", 2, "interrupted()");
}
}
This command is a little different than the ones we created before. In order to successfully intake a ball into the ball holder, in addition to running the intake motors, we will need to run the spinner motors backwards so the ball is sucked down into the holder. This means that this command will need access to both the IntakeSubsystem and the SpinnerSubsystem. We indicate this by replacing the requires(…) line with these two lines:
requires(Robot.m_intakeSubsystem);
requires(Robot.m_spinnerSubsystem);
When this command gets executed we need to start up both the intake and spinner motors, so we add the following two lines to the execute() function:
Robot.m_intakeSubsystem.Intake();
Robot.m_spinnerSubsystem.Intake();
Note that the SpinnerSubsystem does not currently have an Intake() function (only a Shoot() function), so we will need to add that in a bit to get rid of the error we are seeing.
Before we do that, however, lets finish off this command by turning the intake and spinner motors off once the command is complete. Add the following two lines to the interrupted() function:
Robot.m_intakeSubsystem.Stop();
Robot.m_spinnerSubsystem.Stop();
Your IntakeCommand.java file should now look like:
package commands;
import robot.Robot;
import robotCore.Logger;
import wpilib.Command;
/**
*
*/
public class IntakeCommand extends Command
{
public IntakeCommand()
{
Logger.Log("IntakeCommand", 3, "IntakeCommand()");
// Use requires() here to declare subsystem dependencies
requires(Robot.m_intakeSubsystem);
requires(Robot.m_spinnerSubsystem);
}
// Called just before this Command runs the first time
protected void initialize()
{
Logger.Log("IntakeCommand", 2, "initialize()");
Robot.m_intakeSubsystem.Intake();
Robot.m_spinnerSubsystem.Intake();
}
// Called repeatedly when this Command is scheduled to run
protected void execute()
{
Logger.Log("IntakeCommand", -1, "execute()");
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished()
{
Logger.Log("IntakeCommand", -1, "isFinished()");
return (false);
}
// Called once after isFinished returns true
protected void end()
{
Logger.Log("IntakeCommand", 2, "end()");
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted()
{
Logger.Log("IntakeCommand", 2, "interrupted()");
Robot.m_intakeSubsystem.Stop();
Robot.m_spinnerSubsystem.Stop();
}
}
Now, let’s add the needed Intake() function to the SpinnerSubsystem class. Open the SpinnerSubsystem.java file and add the following function:
public void Intake()
{
m_rightSpinner.set(k_intakeSpeed);
m_leftSpinner.set(k_intakeSpeed);
}
Note that we are, once again, using a constant here which we need to define by adding the following line at the top of the class:
private static final double k_intakeSpeed = -0.8;
Note that we are not running the spinner motors full power in reverse. We want them to spin fast enough that they can suck the ball down, but not so fast that it slams down too hard.
Your SpinnerSubsystem.java file should now look like:
package subsystem;
import robotCore.I2CPWMMotor;
import robotCore.Logger;
import wpilib.Subsystem;
/**
*
*/
public class SpinnerSubsystem extends Subsystem
{
private static final double k_intakeSpeed = -0.8;
private I2CPWMMotor m_rightSpinner = null;
private I2CPWMMotor m_leftSpinner = null;
public void initDefaultCommand()
{
Logger.Log("SpinnerSubsystem", 2, "initDefaultCommand()");
}
public void Init()
{
m_rightSpinner = new I2CPWMMotor(6, 7);
m_leftSpinner = new I2CPWMMotor(10, 11);
Logger.Log("SpinnerSubsystem", 2, "Init()");
}
public void SpinUp()
{
m_rightSpinner.set(1.0);
m_leftSpinner.set(1.0);
}
public void Stop()
{
m_rightSpinner.set(0);
m_leftSpinner.set(0);
}
public void Intake()
{
m_rightSpinner.set(k_intakeSpeed);
m_leftSpinner.set(k_intakeSpeed);
}
}
Finally, we need to connect a button to execute this command. Open the OI.java file and define the button we are going to use by inserting the following line at the beginning of your OI class definition:
Button m_intake = new JoystickButton(m_stick, 8);
As you can see we will be using button 8 for this command. Add the following line to the OI() constructor to connect this button to our IntakeCommand.
m_intake.whileHeld(new IntakeCommand());
This will set it so that the intake will run as long as we hold down the intake button (button 8).
Your OI.java file should now look like:
package robot;
import commands.IntakeCommand;
import commands.ShootCommand;
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);
// 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());
}
}
Now deploy and run your program and see if you can pick up the ball.