Acquire gear from the front

This example discusses how one might write a command group for a fairly complex subsystem like a Gear Manipulator.

Design

In this example, the Gear Manipulator is composed of a claw attached to a carriage. That carriage (and the claw) then moves along a track, pulled by a belt with a motor. This track goes to the front and back of the robot. There is an encoder that can track the position of the carriage along the track.

The claw is actually composed of two pieces, the front claw, and the back claw, each of which can open and close separately. When the front claw is open, gears can come in from the front, and the same goes for the back. On the top and bottom of the claw, there are also intake wheels that spin and suck in gears from either side of the claw.

Thus, to grab a gear from the front of the robot, you move the carriage to the front, open the front claw but keep the back claw closed (otherwise the gear would go all the way through) and then start the front intake motors, sucking in the gear.

However, another important thing is that the track cannot move while either claw is open. So, in the beginning, before moving the carriage, you have to close both claws, and wait 0.2 seconds to make sure they’re both closed fully. Otherwise, robot may be damaged. Also, when moving the carriage, stop the intake motors to prevent wear if they drag against the ground.

In this example, the operator wants this whole sequence of events to be bound to the left bumper button.

Subsystem methods

This guide assumes that there already is a GearManiulator subsystem. Additionally, there are also already commmands written for

• moving the carriage to the front (MoveCarriageFront)
• opening and closing the front and back claws (OpenFrontClaw, OpenBackClaw, CloseFrontClaw, CloseBackClaw)
• controlling intake wheels (StartFrontIntake, StartBackIntake, StopFrontIntake, StopBackIntake).

We also assume that OI already has a Joystick named operator, and that the Left Bumper button on the operator’s controller is button 5.

On how to use OI, see OI.

Writing the command

Once you create the FrontAcquire command group, you’ll get a blank command group template like this:

package org.usfirst.frc.team93.robot.commands;

import edu.wpi.first.wpilibj.command.CommandGroup;

/**
 *
 */
public class FrontAcquire extends CommandGroup {

    public FrontAcquire() {
        // Add Commands here:
        // e.g. addSequential(new Command1());
        //      addSequential(new Command2());
        // these will run in order.

        // To run multiple commands at the same time,
        // use addParallel()
        // e.g. addParallel(new Command1());
        //      addSequential(new Command2());
        // Command1 and Command2 will run in parallel.

        // A command group will require all of the subsystems that each member
        // would require.
        // e.g. if Command1 requires chassis, and Command2 requires arm,
        // a CommandGroup containing them would require both the chassis and the
        // arm.
    }
}

In the javadoc at the top, describe what this command does.

/**
 * This command group moves the carriage to the front,
 * opens the front claw, and starts the front intake wheels.
 * It makes it easy for the operator to acquire a gear from the front.
 */

Then, the constructor is where all of the command adding goes.

You can also delete the autogenerated comments in the body of the constructor if you like.

So, the sequence of events is to stop the intake motors, close the front and back claws, wait 0.2 seconds to make sure they fully close, then move the gear to the front, then open the claw and start the intake motors.

Now, your command instead looks like this:

package org.usfirst.frc.team93.robot.commands;

import edu.wpi.first.wpilibj.command.CommandGroup;

/**
 * This command group moves the carriage to the front,
 * opens the front claw, and starts the front intake wheels.
 * It makes it easy for the operator to acquire a gear from the front.
 */
public class FrontAcquire extends CommandGroup
{
    public FrontAcquire()
    {
        addSequential(new StopFrontIntake()); // stop front intake wheels
        addSequential(new StopBackIntake()); // stop back intake wheels

        addSequential(new CloseFrontClaw()); // close back claw
        addSequential(new CloseBackClaw()); // close front claw

        addSequential(new WaitCommand(0.2)); // wait for both claws to close

        addSequential(new MoveCarriageFront()); // move carriage to front now that claws are closed

        addSequential(new OpenFrontClaw()); // open the front claw
        addSequential(new StartFrontIntake()); // start front intake wheels
    }
}

That’s all there is to the command group. It’s done.

Running the command

Now, go into the OI class. We need to set the FrontAcquire command to run whenever the operator presses the left bumper button.

public static Joystick driver;
public static Joystick operator;

public OI()
{
    driver = new Joystick(0);
    operator = new Joystick(1);

    Button frontAcquireButton = new JoystickButton(operator, 5); // left bumper button
    frontAcquireButton.whenPressed(new FrontAcquire());
}

And that’s it! Now, whenever the operator presses left bumper, the robot will try to acquire a gear from the front.

Final code

package org.usfirst.frc.team93.robot.commands;

import edu.wpi.first.wpilibj.command.CommandGroup;

/**
 * This command group moves the carriage to the front,
 * opens the front claw, and starts the front intake wheels.
 * It makes it easy for the operator to acquire a gear from the front.
 */
public class FrontAcquire extends CommandGroup
{
    public FrontAcquire()
    {
        addSequential(new StopFrontIntake()); // stop front intake wheels
        addSequential(new StopBackIntake()); // stop back intake wheels

        addSequential(new CloseFrontClaw()); // close back claw
        addSequential(new CloseBackClaw()); // close front claw

        addSequential(new WaitCommand(0.2)); // wait for both claws to close

        addSequential(new MoveCarriageFront()); // move carriage to front now that claws are closed

        addSequential(new OpenFrontClaw()); // open the front claw
        addSequential(new StartFrontIntake()); // start front intake wheels
    }
}