Command Based Programlama

Java
​
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.command.Subsystem;
import org.usfirst.frc.team1.robot.RobotMap;
​
public class Claw extends Subsystem {
​
	Victor motor = RobotMap.clawMotor;
​
    public void initDefaultCommand() {
    }
​
    public void open() {
    	motor.set(1);
    }
​
    public void close() {
    	motor.set(-1);
    }
​
    public void stop() {
    	motor.set(0);
    }
}
​

​
Java
​
package org.usfirst.frc.team1.robot.commands;
​
import edu.wpi.first.wpilibj.command.Command;
import org.usfirst.frc.team1.robot.Robot;
/**
 *
 */
public class OpenClaw extends Command {
​
    public OpenClaw() {
        requires(Robot.claw);
        setTimeout(.9);
    }
​
    protected void initialize() {
    	Robot.claw.open()
    }
​
    protected void execute() {
    }
​
    protected boolean isFinished() {
        return isTimedOut();
    }
​
    protected void end() {
    	Robot.claw.stop();
    }
​
    protected void interrupted() {
    	end();
    }
}
​

Java
​
package org.usfirst.frc.team1.robot.subsystems;
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import org.usfirst.frc.team1.robot.RobotMap;
​
​
public class Wrist extends PIDSubsystem { // This system extends PIDSubsystem
​
	Victor motor = RobotMap.wristMotor;
	AnalogInput pot = RobotMap.wristPot();
​
	public Wrist() {
		super("Wrist", 2.0, 0.0, 0.0);// The constructor passes a name for the subsystem and the P, I and D constants that are used when computing the motor output
		setAbsoluteTolerance(0.05);
		getPIDController().setContinuous(false);
	}
	
    public void initDefaultCommand() {
    }
​
    protected double returnPIDInput() {
    	return pot.getAverageVoltage(); // returns the sensor value that is providing the feedback for the system
    }
​
    protected void usePIDOutput(double output) {
    	motor.pidWrite(output); // this is where the computed output value fromthe PIDController is applied to the motor
    }
}
​

C++
​
#include "MyCommandName.h"
​
/*
	 * 1.	Constructor - Might have parameters for this command such as target positions of devices. Should also set the name of the command for debugging purposes.
	 *  This will be used if the status is viewed in the dashboard. And the command should require (reserve) any devices is might use.
	 */
MyCommandName::MyCommandName() : CommandBase("MyCommandName") 
{
	Requires(Elevator);
}
​
// initialize() - This method sets up the command and is called immediately before the command is executed for the first time and 
// every subsequent time it is started . Any initialization code should be here.
void MyCommandName::Initialize()
{
​
}
​
/*
 *	execute() - This method is called periodically (about every 20ms) and does the work of the command. Sometimes, if there is a position a
 *  subsystem is moving to, the command might set the target position for the subsystem in initialize() and have an empty execute() method.
 */
void MyCommandName::Execute()
{
​
}
​
bool MyCommandName::IsFinished()
{
	return false;
}
​
void MyCommandName::End()
{
​
}
​
// Make this return true when this Command no longer needs to run execute()
void MyCommandName::Interrupted()
{
​
}
​

Java
​
public class MyCommandName extends Command {
​
	/*
	 * 1.	Constructor - Might have parameters for this command such as target positions of devices. Should also set the name of the command for debugging purposes.
	 *  This will be used if the status is viewed in the dashboard. And the command should require (reserve) any devices is might use.
	 */
    public MyCommandName() {
    	super("MyCommandName");
        requires(elevator);
    }
​
    // 	initialize() - This method sets up the command and is called immediately before the command is executed for the first time and every subsequent time it is started .
    //  Any initialization code should be here. 
    protected void initialize() {
    }
​
    /*
     *	execute() - This method is called periodically (about every 20ms) and does the work of the command. Sometimes, if there is a position a
     *  subsystem is moving to, the command might set the target position for the subsystem in initialize() and have an empty execute() method.
     */
    protected void execute() {
    }
​
    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return false;
    }
}
​

C++
​
#include "DriveWithJoysticks.h"
#include "RobotMap.h"
​
DriveWithJoysticks::DriveWithJoysticks() : CommandBase("DriveWithJoysticks")
{
	Requires(Robot::drivetrain); // Drivetrain is our instance of the drive system
}
​
// Called just before this Command runs the first time
void DriveWithJoysticks::Initialize()
{
​
}
​
    /*
     * execute() - In our execute method we call a tankDrive method we have created in our subsystem. This method takes two speeds as a parameter which we get from methods in the OI class.
     * These methods abstract the joystick objects so that if we want to change how we get the speed later we can do so without modifying our commands
     * (for example, if we want the joysticks to be less sensitive, we can multiply them by .5 in the getLeftSpeed method and leave our command the same).
     */
void DriveWithJoysticks::Execute()
{
	Robot::drivetrain->Drive(Robot::oi->GetJoystick());
}
​
    /*
     * isFinished - Our isFinished method always returns false meaning this command never completes on it's own. The reason we do this is that this command will be set as the default command for the subsystem. This means that whenever the subsystem is not running another command, it will run this command. If any other command is scheduled it will interrupt this command, then return to this command when the other command completes.
     */
bool DriveWithJoysticks::IsFinished()
{
	return false;
}
​
void DriveWithJoysticks::End()
{
	Robot::drivetrain->Drive(0, 0);
}
​
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
void DriveWithJoysticks::Interrupted()
{
	End();
}
​

Java
​
public class DriveWithJoysticks extends Command {
​
    public DriveWithJoysticks() {
    	requires(drivetrain);// drivetrain is an instance of our Drivetrain subsystem
    }
​
    protected void initialize() {
    }
​
    /*
     * execute() - In our execute method we call a tankDrive method we have created in our subsystem. This method takes two speeds as a parameter which we get from methods in the OI class.
     * These methods abstract the joystick objects so that if we want to change how we get the speed later we can do so without modifying our commands
     * (for example, if we want the joysticks to be less sensitive, we can multiply them by .5 in the getLeftSpeed method and leave our command the same).
     */
    protected void execute() {
    	drivetrain.tankDrive(oi.getLeftSpeed(), oi.getRightSpeed());
    }
​
    /*
     * isFinished - Our isFinished method always returns false meaning this command never completes on it's own. The reason we do this is that this command will be set as the default command for the subsystem. This means that whenever the subsystem is not running another command, it will run this command. If any other command is scheduled it will interrupt this command, then return to this command when the other command completes.
     */
    protected boolean isFinished() {
        return false;
    }
​
    protected void end() {
    }
​
    protected void interrupted() {
    }
}
​

C++
​
#include "PlaceSoda.h"
​
PlaceSoda::PlaceSoda()
{
	AddSequential(new SetElevatorSetpoint(TABLE_HEIGHT));
	AddSequential(new SetWristSetpoint(PICKUP));
	AddSequential(new OpenClaw());
}
​
​

Java
​
public class PlaceSoda extends CommandGroup {
​
    public  PlaceSoda() {
    	addSequential(new SetElevatorSetpoint(Elevator.TABLE_HEIGHT));
    	addSequential(new SetWristSetpoint(Wrist.PICKUP));
    	addSequential(new OpenClaw());
    }
}
​

C++
​
#include "PrepareToGrab.h"
​
PrepareToGrab::PrepareToGrab()
{
	AddParallel(new SetWristSetpoint(PICKUP));
	AddParallel(new SetElevatorSetpoint(BOTTOM));
	AddParallel(new OpenClaw());
}
​

Java
​
public class PrepareToGrab extends CommandGroup {
​
    public  PrepareToGrab() {
    	addParallel(new SetWristSetpoint(Wrist.PICKUP));
    	addParallel(new SetElevatorSetpoint(Elevator.BOTTOM));
    	addParallel(new OpenClaw());
    }
}

C++
​
#include "Grab.h"
​
Grab::Grab()
{
	AddSequential(new CloseClaw());
	AddParallel(new SetElevatorSetpoint(STOW));
	AddSequential(new SetWristSetpoint(STOW));
}
​

Java
​
public class Grab extends CommandGroup {
​
    public  Grab() {
    	addSequential(new CloseClaw());
    	addParallel(new SetElevatorSetpoint(Elevator.STOW));
    	addSequential(new SetWristSetpoint(Wrist.STOW));
    }
}

C++
​
OI::OI()
{
	joy = new Joystick(1);
​
	JoystickButton* button1 = new JoystickButton(joy, 1),
			button2 = new JoystickButton(joy, 2),
			button3 = new JoystickButton(joy, 3),
			button4 = new JoystickButton(joy, 4),
			button5 = new JoystickButton(joy, 5),
			button6 = new JoystickButton(joy, 6),
			button7 = new JoystickButton(joy, 7),
			button8 = new JoystickButton(joy, 8);
	
}
​

Java
​
public class OI {
    // Create the joystick and the 8 buttons on it
	Joystick leftJoy = new Joystick(1);
	Button button1 = new JoystickButton(leftJoy, 1),
			button2 = new JoystickButton(leftJoy, 2),
			button3 = new JoystickButton(leftJoy, 3),
			button4 = new JoystickButton(leftJoy, 4),
			button5 = new JoystickButton(leftJoy, 5),
			button6 = new JoystickButton(leftJoy, 6),
			button7 = new JoystickButton(leftJoy, 7),
			button8 = new JoystickButton(leftJoy, 8);
	
}

C++
​
	button1->WhenPressed(new PrepareToGrab());
	button2->WhenPressed(new Grab());
	button3->WhenPressed(new DriveToDistance(0.11));
	button4->WhenPressed(new PlaceSoda());
	button6->WhenPressed(new DriveToDistance(0.2));
	button8->WhenPressed(new Stow());
	
	button7->WhenPressed(new SodaDelivery());
​

Java
​
	public OI() {
		button1.whenPressed(new PrepareToGrab());
		button2.whenPressed(new Grab());
		button3.whenPressed(new DriveToDistance(0.11));
		button4.whenPressed(new PlaceSoda());
		button6.whenPressed(new DriveToDistance(0.2));
		button8.whenPressed(new Stow());
		
		button7.whenPressed(new SodaDelivery());
	}
​

C++
​
	button1->WhenPressed(new PrepareToGrab());
	button2->WhenPressed(new Grab());
	button3->WhenPressed(new DriveToDistance(0.11));
	button4->WhenPressed(new PlaceSoda());
	button6->WhenPressed(new DriveToDistance(0.2));
	button8->WhenPressed(new Stow());
	
	button7->WhenPressed(new SodaDelivery());
​

Java
​
	public OI() {
		button1.whenPressed(new PrepareToGrab());
		button2.whenPressed(new Grab());
		button3.whenPressed(new DriveToDistance(0.11));
		button4.whenPressed(new PlaceSoda());
		button6.whenPressed(new DriveToDistance(0.2));
		button8.whenPressed(new Stow());
		
		button7.whenPressed(new SodaDelivery());
	}
​

C++
​
Command* autonomousCommand;
​
class Robot: public IterativeRobot {
​
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
	void RobotInit()
	{
        // instantiate the command used for the autonomous period
		autonomousCommand = new SodaDelivery();
		oi = new OI();
​
	}
	
​
	void AutonomousInit()
	{
        // schedule the autonomous command (example)
		if(autonomousCommand != NULL) autonomousCommand->Start();
	}
	/*
	 * This function is called periodically during autonomous
	 */
	void AutonomousPeriodic()
	{
		Scheduler::GetInstance()->Run();
	}
​

Java
​
public class Robot extends IterativeRobot {
​
​
​
    Command autonomousCommand;
​
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
		oi = new OI();
        // instantiate the command used for the autonomous period
        autonomousCommand = new SodaDelivery();
    }
​
    public void autonomousInit() {
        // schedule the autonomous command (example)
        if (autonomousCommand != null) autonomousCommand.start();
    }
​
    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
        Scheduler.getInstance().run();
    }

C++
​
// Aim shooter
SetTargetAngle(); // Initialization: prepares for the action to be performed
while (!AtRightAngle()) { // Condition: keeps the loop going while it is satisfied
	CorrectAngle(); // Execution: repeatedly updates the code to try to make the condition false
	delay(); // Delay to prevent maxing CPU
}
HoldAngle(); // End: performs any cleanup and final task before moving on to the next action
​
// Spin up to Speed
SetTargetSpeed(); // Initialization: prepares for the action to be performed
while (!FastEnough()) { // Condition: keeps the loop going while it is satisfied
	SpeedUp(); // Execution: repeatedly updates the code to try to make the condition false
	delay(); // Delay to prevent maxing CPU
}
HoldSpeed();
​
// Shoot Frisbee
Shoot(); // End: performs any cleanup and final task before moving on to the next action
}
​

Java
​
// Aim shooter
SetTargetAngle(); // Initialization: prepares for the action to be performed
while (!AtRightAngle()) { // Condition: keeps the loop going while it is satisfied
	CorrectAngle(); // Execution: repeatedly updates the code to try to make the condition false
	delay(); // Delay to prevent maxing CPU
}
HoldAngle(); // End: performs any cleanup and final task before moving on to the next action
​
// Spin up to Speed
SetTargetSpeed(); // Initialization: prepares for the action to be performed
while (!FastEnough()) { // Condition: keeps the loop going while it is satisfied
	SpeedUp(); // Execution: repeatedly updates the code to try to make the condition false
	delay(); // Delay to prevent maxing CPU
}
HoldSpeed();
​
// Shoot Frisbee
Shoot(); // End: performs any cleanup and final task before moving on to the next action
}

C++
​
#include "AutonomousCommand.h"
​
AutonomousCommand::AutonomousCommand()
{
     AddSequential(new Aim());
     AddSequential(new SpinUpShooter());
     AddSequential(new Shoot());
}
​

Java
​
public class AutonomousCommand extends CommandGroup {
​
    public  AutonomousCommand() {
    	addSequential(new Aim());
    	addSequential(new SpinUpShooter());
    	addSequential(new Shoot());
    }
}
​

C++
​
#include "Aim.h"
​
Aim::Aim()
{
     Requires(Robot::turret);
}
​
// Called just before this Command runs the first time
void Aim::Initialize()
{
     SetTargetAngle();
}
​
// Called repeatedly when this Command is scheduled to run
void Aim:Execute()
{
 \    CorrectAngle();
}
​
// Make this return true when this Command no longer needs to run execute()
bool Aim:IsFinished()
{
     return AtRightAngle();
}
​
// Called once after isFinished returns true
void Aim::End()
{
     HoldAngle();
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
void Aim:Interrupted()
{
     End();
}
​

Java
​
public class Aim extends Command {
​
    public Aim() {
    	requires(Robot.turret);
    }
​
    // Called just before this Command runs the first time
    protected void initialize() {
    	SetTargetAngle();
    }
​
    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	CorrectAngle();
 \   }
​
    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return AtRightAngle();
 \   }
​
    // Called once after isFinished returns true
    protected void end() {
    	HoldAngle();
    }
​
    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    	end();
    }
}
​

C++
​
#include "SpinUpShooter.h"
​
SpinUpShooter::SpinUpShooter()
{
     Requires(Robot::shooter)
}
​
// Called just before this Command runs the first time
void SpinUpShooter::Initialize()
{
 \    SetTargetSpeed();
}
​
// Called repeatedly when this Command is scheduled to run
void SpinUpShooter::Execute()
{
     SpeedUp();
}
​
// Make this return true when this Command no longer needs to run execute()
bool SpinUpShooter::IsFinished()
{
 \    return FastEnough();
}
​
// Called once after isFinished returns true
void SpinUpShooter::End()
{
     HoldSpeed();
}
​
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
void SpinUpShooter::Interrupted()
{
     End();
}
​

Java
​
public class SpinUpShooter extends Command {
​
    public SpinUpShooter() {
        requires(Robot.shooter);
 \   }
​
    // Called just before this Command runs the first time
    protected void initialize() {
    	SetTargetSpeed();
    }
​
    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	SpeedUp();
 \   }
​
    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return FastEnough();
 \   }
​
    // Called once after isFinished returns true
    protected void end() {
    	HoldSpeed();
    }
​
    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    	end();
    }
}
​