public class Cock extends Command {
    private int i;

    public Cock() {
	// Use requires() here to declare subsystem dependencies
	// eg. requires(chassis);
	requires(Robot.shooter);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
	i = 0;
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
	Robot.shooter.setSpeed(.6); // Retract spring
	if (i > 10) {
	    Robot.shooter.insertPin();
	}
	i++;
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
	return (i / 50) > 5; // Time out after 5 seconds
    }

    // Called once after isFinished returns true
    protected void end() {
	Robot.shooter.stop();
	Robot.shooter.setIsCocked(true);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
	Robot.shooter.stop();
	Robot.shooter.setIsCocked(true);
    }
} public class DriveWithJoysticks extends Command {

    public DriveWithJoysticks() {
	// Use requires() here to declare subsystem dependencies
	// eg. requires(chassis);
	requires(Robot.drive);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
	Robot.drive.setLeftSpeed(-Robot.oi.leftStick.getY());
	Robot.drive.setRightSpeed(-Robot.oi.rightStick.getY());
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
	return false;
    }

    // Called once after isFinished returns true
    protected void end() {
	Robot.drive.stop();
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
	Robot.drive.stop();
    }
} public class Eject extends Command {

    public Eject() {
	// Use requires() here to declare subsystem dependencies
	// eg. requires(chassis);
	requires(Robot.intake);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
	Robot.intake.eject();
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
	return false;
    }

    // Called once after isFinished returns true
    protected void end() {
	Robot.intake.stop();
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
	end();
    }
} public class HangerUp extends Command {

    public HangerUp() {
	// Use requires() here to declare subsystem dependencies
	// eg. requires(chassis);
	requires(Robot.hanger);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
	Robot.hanger.setSpeed(1);
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
	return false;
    }

    // Called once after isFinished returns true
    protected void end() {
	Robot.hanger.stop();
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
	end();
    }
} public class Intake extends Command {

    public Intake() {
	// Use requires() here to declare subsystem dependencies
	// eg. requires(chassis);
	requires(Robot.intake);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
	if (Robot.intake.getLimit()) {
	    Robot.intake.stop();
	} else {
	    Robot.intake.intake();
	}
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
	return false;
    }

    // Called once after isFinished returns true
    protected void end() {
	Robot.intake.stop();
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
	Robot.intake.stop();
    }
} public class IntakeOverride extends Command {

    public IntakeOverride() {
	// Use requires() here to declare subsystem dependencies
	// eg. requires(chassis);
	requires(Robot.intake);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
	Robot.intake.set(1);
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
	return false;
    }

    // Called once after isFinished returns true
    protected void end() {
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }
} public class ShiftDown extends Command {

    public ShiftDown() {
	// Use requires() here to declare subsystem dependencies
	// eg. requires(chassis);
	requires(Robot.drive);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
	Robot.drive.setSol(-1);
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
	return true;
    }

    // Called once after isFinished returns true
    protected void end() {
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }
} public class ShiftUp extends Command {

    public ShiftUp() {
	// Use requires() here to declare subsystem dependencies
	// eg. requires(chassis);
	requires(Robot.drive);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
	Robot.drive.setSol(1);
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
	return true;
    }

    // Called once after isFinished returns true
    protected void end() {
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }
} public class ShootCycle extends CommandGroup {
    public ShootCycle() {
	addSequential(new Shoot());
	addSequential(new Wait(0.0D));
	addSequential(new Cock());
    }
} public class ArmWithJoystick extends Command {
    private final double scaleFactorUp = 1.5;
    private final double scaleFactorDown = 4;
    private final int axis = ControlMap.Controller.AXIS_ARM;

    public ArmWithJoystick() {
	// Use requires() here to declare subsystem dependencies
	// eg. requires(chassis);
	requires(Robot.arm);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
	Robot.arm.disable();
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
	double joySpeed = -Robot.oi.controller.getRawAxis(axis);

	if (joySpeed / joySpeed == 1) {
	    // Arm is going up
	    Robot.arm.setSpeed(joySpeed / scaleFactorUp);
	} else {
	    // Arm is going down
	    Robot.arm.setSpeed(joySpeed / scaleFactorDown);
	}

	if (Robot.arm.getBottomLimit()) {
	    Robot.arm.zeroEncoder();
	}
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
	return false;
    }

    // Called once after isFinished returns true
    protected void end() {
	Robot.arm.setSetpointRelative(0); // Keep arm at this angle
	Robot.arm.enable();
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
	end();
    }
} public class GoBottomTrust extends Command {
    private int i;

    public GoBottomTrust() {
	// Use requires() here to declare subsystem dependencies
	// eg. requires(chassis);
	requires(Robot.arm);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
	i = 0;
	Robot.arm.disable();
	System.out.print("Arm coming down");
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
	if (Robot.arm.getRawEncoder() > 60) {
	    Robot.arm.setSpeed(-0.25);
	} else {
	    Robot.arm.setSpeed(-0.1);
	}
	i++;
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
	int seconds = i / 50;
	return (Robot.arm.getRawEncoder() < 10 && seconds > 3) || Robot.arm.getBottomLimit();
    }

    // Called once after isFinished returns true
    protected void end() {
	Robot.arm.stop();
	Robot.arm.setSetpointRelative(0);
	Robot.arm.enable();
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }
} public class SetArmAngle extends Command {
    private double goal;
    private final double margin = 20;

    public SetArmAngle(double _goal) {
	// Use requires() here to declare subsystem dependencies
	// eg. requires(chassis);
	requires(Robot.arm);
	this.goal = _goal;
    }

    // Called just before this Command runs the first time
    protected void initialize() {
	Robot.arm.setSetpoint(goal);
	Robot.arm.enable();
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
	// TODO ask for low speed
	return Math.abs(Robot.arm.getPIDController().getError()) < margin;
    }

    // Called once after isFinished returns true
    protected void end() {
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }
}
import org.usfirst.frc.team236.robot.commands.Cock;
import org.usfirst.frc.team236.robot.commands.DriveWithJoysticks;
import org.usfirst.frc.team236.robot.commands.Eject;
import org.usfirst.frc.team236.robot.commands.HangerUp;
import org.usfirst.frc.team236.robot.commands.Intake;
import org.usfirst.frc.team236.robot.commands.IntakeOverride;
import org.usfirst.frc.team236.robot.commands.ShiftDown;
import org.usfirst.frc.team236.robot.commands.ShiftUp;
import org.usfirst.frc.team236.robot.commands.ShootCycle;
import org.usfirst.frc.team236.robot.commands.arm.ArmWithJoystick;
import org.usfirst.frc.team236.robot.commands.arm.GoBottomTrust;
import org.usfirst.frc.team236.robot.commands.arm.SetArmAngle;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.buttons.Button;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
package org.usfirst.frc.team236.robot;



public class OI {

    public Joystick leftStick;
    public Joystick rightStick;
    public Joystick controller;
    public Button shiftUp;
    public Button shiftDown;
    public Button intake;
    public Button eject;
    public Button shoot;
    public Button cock;
    public Button manualArmUp;
    public Button manualArmDown;
    public Button armWithJoystick;
    public Button invertDrive;
    public Button normalDrive;
    public Button armWithPOV;
    public Button armPIDTest;
    public Button goBottom;
    public Button goNearBatterHighShotAngle;
    public Button goDefenseHighShotAngle;
    public Button goFarBatterHighShotAngle;
    public Button controllerShoot;
    public Button controllerIntake;
    public Button intakeOverride;
    public Button hangerUp;
    public Button fixDrive;

    public OI() {

	// Ports
	leftStick = new Joystick(ControlMap.PORT_STICK_LEFT);
	rightStick = new Joystick(ControlMap.PORT_STICK_RIGHT);
	controller = new Joystick(ControlMap.PORT_CONTROLLER);

	// Right Stick
	shiftDown = new JoystickButton(rightStick, ControlMap.RightStick.SHIFT_DOWN);
	shiftDown.whenPressed(new ShiftDown());

	shiftUp = new JoystickButton(rightStick, ControlMap.RightStick.SHIFT_UP);
	shiftUp.whenPressed(new ShiftUp());

	// Left Stick
	eject = new JoystickButton(leftStick, ControlMap.LeftStick.EJECT);
	eject.whileHeld(new Eject());

	intake = new JoystickButton(leftStick, ControlMap.LeftStick.INTAKE);
	intake.whileHeld(new Intake());