<commit_before>package com.team649.frc2014.commands;

import com.sun.squawk.debugger.sda.SDA;
import com.team649.frc2014.OI;
import com.team649.frc2014.RobotMap;
//Why are we importing something from this class into this class
//import com.team649.frc2014.commands.CommandBase.shootBall;
import com.team649.frc2014.commands.drivetrain.DriveForwardRotate;
import com.team649.frc2014.commands.drivetrain.DriveSetDistanceWithPIDCommand;
import com.team649.frc2014.commands.fingers.SetFingerPosition;
import com.team649.frc2014.commands.pivot.ManualDriveClawPivot;
import com.team649.frc2014.commands.pivot.SetClawPosition;
import com.team649.frc2014.commands.rollers.RunRollers;
import com.team649.frc2014.commands.winch.AutoCoilClawWinch;
import com.team649.frc2014.commands.winch.ManualCoilClawWinch;
import com.team649.frc2014.commands.winch.SetClawWinchSolenoid;
import com.team649.frc2014.subsystems.CameraSubsystem;
import com.team649.frc2014.subsystems.ClawFingerSubsystem;
import com.team649.frc2014.subsystems.ClawPivotSubsystem;
import com.team649.frc2014.subsystems.ClawRollerSubsystem;
import com.team649.frc2014.subsystems.ClawWinchSubsystem;
import com.team649.frc2014.subsystems.DriveTrainSubsystem;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.CommandGroup;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 * The base for all commands. All atomic commands should subclass CommandBase. CommandBase stores creates and stores each control system. To access a subsystem elsewhere in your code in your code use
 * CommandBase.exampleSubsystem
 *
 * @author Author
 */
public abstract class CommandBase extends Command {

    public static OI oi;
    // Create a single static instance of all of your subsystems
    public static DriveTrainSubsystem driveTrainSubsystem = new DriveTrainSubsystem();
    public static CameraSubsystem cameraSubsystem = new CameraSubsystem();
    public static ClawPivotSubsystem clawPivotSubsystem = new ClawPivotSubsystem();
    public static ClawWinchSubsystem clawWinchSubsystem = new ClawWinchSubsystem();
    public static ClawFingerSubsystem clawFingerSubsystem = new ClawFingerSubsystem();
    public static ClawRollerSubsystem clawRollerSubsystem = new ClawRollerSubsystem();
    private static Compressor compressor;

    public static void init() {
        compressor = new Compressor(RobotMap.PRESSURE_SWITCH_CHANNEL, RobotMap.COMPRESSOR_RELAY_CHANNEL);
        compressor.start();
        oi = new OI();
    }

    public static Command driveForwardRotate(double driveForward, double driveRotation) {
        return new DriveForwardRotate(driveForward, driveRotation);
    }

    public static Command manualCoilClawWinch() {
        return new ManualCoilClawWinch();
    }

    public static Command autoCoilClawWinch() {
        return new AutoCoilClawWinch();
    }

    public static boolean isCompressorRunning() {
        return !compressor.getPressureSwitchValue();
    }

    public CommandBase(String name) {
        super(name);
    }

    public CommandBase() {
        super();

    }
    public static Command shootHotGoalShortDriveAutonomous() {
        CommandGroup driveAndCheckGoal = driveAndPrepareToShoot(true, DriveTrainSubsystem.EncoderBasedDriving.AUTONOMOUS_DRIVE_DISTANCE_SHORT);
        CommandGroup mainAutonomousSequence = new CommandGroup("mainAutoSeq");
        //drive and check goal. When both are done (checking goal and driving), shoot
        mainAutonomousSequence.addSequential(setFingerPosition(ClawFingerSubsystem.DOWN));
        mainAutonomousSequence.addSequential(new SetClawWinchSolenoid(true));
        mainAutonomousSequence.addSequential(driveAndCheckGoal);
        mainAutonomousSequence.addSequential(new WaitCommand(300));
        mainAutonomousSequence.addSequential(shootBall());
        return mainAutonomousSequence;
    }

   public static Command twoBallShortDriveAutonomous() {
        CommandGroup mainAutonomousSequence = new CommandGroup("mainAutoSeq");
        mainAutonomousSequence.addSequential(driveAndPrepareToShoot(false, DriveTrainSubsystem.EncoderBasedDriving.AUTONOMOUS_DRIVE_DISTANCE_SHORT, true));
        mainAutonomousSequence.addSequential(new WaitCommand(300));
        mainAutonomousSequence.addSequential(shootBall(false));
        mainAutonomousSequence.addParallel(autoCoilClawWinch(), ClawWinchSubsystem.MAX_COIL_TIME);
        mainAutonomousSequence.addSequential(repositionAndPickup(DriveTrainSubsystem.EncoderBasedDriving.AUTONOMOUS_DRIVE_DISTANCE_SHORT));
        mainAutonomousSequence.addParallel(realignBall());
        mainAutonomousSequence.addSequential(driveAndPrepareToShoot(false, DriveTrainSubsystem.EncoderBasedDriving.AUTONOMOUS_DRIVE_DISTANCE_SHORT - 24, true));
        mainAutonomousSequence.addSequential(new WaitCommand(300));
        mainAutonomousSequence.addSequential(shootBall());
        return mainAutonomousSequence;
    }

    private static CommandGroup repositionAndPickup(double driveDistance) {
        CommandGroup repositionAndPickup = new CommandGroup();
        //Extra 12 inches might be unnesscary, check that
        repositionAndPickup.addParallel(new SetClawPosition(ClawPivotSubsystem.PICKUP));
        repositionAndPickup.addSequential(new WaitCommand(500));

        repositionAndPickup.addSequential(new RunRollers(ClawRollerSubsystem.ROLLER_SPIN_INTAKE_SPEED));
        repositionAndPickup.addSequential(new DriveSetDistanceWithPIDCommand(-driveDistance + 16, 0.63));

        return repositionAndPickup;
    }
    private static CommandGroup realignBall() {
        CommandGroup realign = new CommandGroup();
        realign.addSequential(new RunRollers(ClawRollerSubsystem.ROLLER_SPIN_REALIGN_SPEED));
        realign.addSequential(new WaitCommand(1000));
        realign.addSequential(new RunRollers(ClawRollerSubsystem.ROLLER_SPIN_OFF_SPEED));
        return realign;
    }
    
    private static CommandGroup driveAndPrepareToShoot(boolean checkHot, double driveDistance, boolean keepPidRunning) {
        CommandGroup driveAndCheckGoal = new CommandGroup("driveAndCheck");
        driveAndCheckGoal.addParallel(setFingerPosition(ClawFingerSubsystem.DOWN));
        driveAndCheckGoal.addParallel(new SetClawWinchSolenoid(true));
        if (!keepPidRunning) {
            driveAndCheckGoal.addParallel(new DriveSetDistanceWithPIDCommand(driveDistance, .6));
            driveAndCheckGoal.addParallel(new SetClawPosition(ClawPivotSubsystem.BACKWARD_SHOOT));
        } else {
            ChangeableBoolean bool = new ChangeableBoolean(false);
            driveAndCheckGoal.addParallel(new DriveSetDistanceWithPIDCommand(driveDistance, .5, bool));
            driveAndCheckGoal.addParallel(new SetClawPosition(ClawPivotSubsystem.BACKWARD_SHOOT, bool));
        }
//        check the hot goal after .5 seconds
        if (checkHot) {
            CommandGroup checkHotGoal = new CommandGroup("checkHotGoal");
            checkHotGoal.addSequential(new WaitCommand(1000));<commit_msg>changed some small auto timings<commit_after>package com.team649.frc2014.commands;

import com.sun.squawk.debugger.sda.SDA;
import com.team649.frc2014.OI;
import com.team649.frc2014.RobotMap;
//Why are we importing something from this class into this class
//import com.team649.frc2014.commands.CommandBase.shootBall;
import com.team649.frc2014.commands.drivetrain.DriveForwardRotate;
import com.team649.frc2014.commands.drivetrain.DriveSetDistanceWithPIDCommand;
import com.team649.frc2014.commands.fingers.SetFingerPosition;
import com.team649.frc2014.commands.pivot.ManualDriveClawPivot;
import com.team649.frc2014.commands.pivot.SetClawPosition;
import com.team649.frc2014.commands.rollers.RunRollers;
import com.team649.frc2014.commands.winch.AutoCoilClawWinch;
import com.team649.frc2014.commands.winch.ManualCoilClawWinch;
import com.team649.frc2014.commands.winch.SetClawWinchSolenoid;
import com.team649.frc2014.subsystems.CameraSubsystem;
import com.team649.frc2014.subsystems.ClawFingerSubsystem;
import com.team649.frc2014.subsystems.ClawPivotSubsystem;
import com.team649.frc2014.subsystems.ClawRollerSubsystem;
import com.team649.frc2014.subsystems.ClawWinchSubsystem;
import com.team649.frc2014.subsystems.DriveTrainSubsystem;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.CommandGroup;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 * The base for all commands. All atomic commands should subclass CommandBase. CommandBase stores creates and stores each control system. To access a subsystem elsewhere in your code in your code use
 * CommandBase.exampleSubsystem
 *
 * @author Author
 */
public abstract class CommandBase extends Command {

    public static OI oi;
    // Create a single static instance of all of your subsystems
    public static DriveTrainSubsystem driveTrainSubsystem = new DriveTrainSubsystem();
    public static CameraSubsystem cameraSubsystem = new CameraSubsystem();
    public static ClawPivotSubsystem clawPivotSubsystem = new ClawPivotSubsystem();
    public static ClawWinchSubsystem clawWinchSubsystem = new ClawWinchSubsystem();
    public static ClawFingerSubsystem clawFingerSubsystem = new ClawFingerSubsystem();
    public static ClawRollerSubsystem clawRollerSubsystem = new ClawRollerSubsystem();
    private static Compressor compressor;

    public static void init() {
        compressor = new Compressor(RobotMap.PRESSURE_SWITCH_CHANNEL, RobotMap.COMPRESSOR_RELAY_CHANNEL);
        compressor.start();
        oi = new OI();
    }

    public static Command driveForwardRotate(double driveForward, double driveRotation) {
        return new DriveForwardRotate(driveForward, driveRotation);
    }

    public static Command manualCoilClawWinch() {
        return new ManualCoilClawWinch();
    }

    public static Command autoCoilClawWinch() {
        return new AutoCoilClawWinch();
    }

    public static boolean isCompressorRunning() {
        return !compressor.getPressureSwitchValue();
    }

    public CommandBase(String name) {
        super(name);
    }

    public CommandBase() {
        super();

    }
    public static Command shootHotGoalShortDriveAutonomous() {
        CommandGroup driveAndCheckGoal = driveAndPrepareToShoot(true, DriveTrainSubsystem.EncoderBasedDriving.AUTONOMOUS_DRIVE_DISTANCE_SHORT);
        CommandGroup mainAutonomousSequence = new CommandGroup("mainAutoSeq");
        //drive and check goal. When both are done (checking goal and driving), shoot
        mainAutonomousSequence.addSequential(setFingerPosition(ClawFingerSubsystem.DOWN));
        mainAutonomousSequence.addSequential(new SetClawWinchSolenoid(true));
        mainAutonomousSequence.addSequential(driveAndCheckGoal);
        mainAutonomousSequence.addSequential(new WaitCommand(300));
        mainAutonomousSequence.addSequential(shootBall());
        return mainAutonomousSequence;
    }

   public static Command twoBallShortDriveAutonomous() {
        CommandGroup mainAutonomousSequence = new CommandGroup("mainAutoSeq");
        mainAutonomousSequence.addSequential(driveAndPrepareToShoot(false, DriveTrainSubsystem.EncoderBasedDriving.AUTONOMOUS_DRIVE_DISTANCE_SHORT, true));
        mainAutonomousSequence.addSequential(new WaitCommand(200));
        mainAutonomousSequence.addSequential(shootBall(false));
        mainAutonomousSequence.addParallel(autoCoilClawWinch(), ClawWinchSubsystem.MAX_COIL_TIME);
        mainAutonomousSequence.addSequential(repositionAndPickup(DriveTrainSubsystem.EncoderBasedDriving.AUTONOMOUS_DRIVE_DISTANCE_SHORT));
        mainAutonomousSequence.addParallel(realignBall());
        mainAutonomousSequence.addSequential(driveAndPrepareToShoot(false, DriveTrainSubsystem.EncoderBasedDriving.AUTONOMOUS_DRIVE_DISTANCE_SHORT - 24, true));
        mainAutonomousSequence.addSequential(new WaitCommand(300));
        mainAutonomousSequence.addSequential(shootBall());
        return mainAutonomousSequence;
    }

    private static CommandGroup repositionAndPickup(double driveDistance) {
        CommandGroup repositionAndPickup = new CommandGroup();
        //Extra 12 inches might be unnesscary, check that
        repositionAndPickup.addParallel(new SetClawPosition(ClawPivotSubsystem.PICKUP));
        repositionAndPickup.addSequential(new WaitCommand(300));

        repositionAndPickup.addSequential(new RunRollers(ClawRollerSubsystem.ROLLER_SPIN_INTAKE_SPEED));
        repositionAndPickup.addSequential(new DriveSetDistanceWithPIDCommand(-driveDistance + 16, 0.63));

        return repositionAndPickup;
    }
    private static CommandGroup realignBall() {
        CommandGroup realign = new CommandGroup();
        realign.addSequential(new RunRollers(ClawRollerSubsystem.ROLLER_SPIN_REALIGN_SPEED));
        realign.addSequential(new WaitCommand(1000));
        realign.addSequential(new RunRollers(ClawRollerSubsystem.ROLLER_SPIN_OFF_SPEED));
        return realign;
    }
    
    private static CommandGroup driveAndPrepareToShoot(boolean checkHot, double driveDistance, boolean keepPidRunning) {
        CommandGroup driveAndCheckGoal = new CommandGroup("driveAndCheck");
        driveAndCheckGoal.addParallel(setFingerPosition(ClawFingerSubsystem.DOWN));
        driveAndCheckGoal.addParallel(new SetClawWinchSolenoid(true));
        if (!keepPidRunning) {
            driveAndCheckGoal.addParallel(new DriveSetDistanceWithPIDCommand(driveDistance, .6));
            driveAndCheckGoal.addParallel(new SetClawPosition(ClawPivotSubsystem.BACKWARD_SHOOT));
        } else {
            ChangeableBoolean bool = new ChangeableBoolean(false);
            driveAndCheckGoal.addParallel(new DriveSetDistanceWithPIDCommand(driveDistance, .5, bool));
            driveAndCheckGoal.addParallel(new SetClawPosition(ClawPivotSubsystem.BACKWARD_SHOOT, bool));
        }
//        check the hot goal after .5 seconds
        if (checkHot) {
            CommandGroup checkHotGoal = new CommandGroup("checkHotGoal");
            checkHotGoal.addSequential(new WaitCommand(1000));
{'fraction_non_alphanumeric': 0.07542097070892882, 'fraction_numerical': 0.02334795528512806, 'mean_word_length': 4.776460972619534, 'pattern_counts': {'":': 0, '<': 3, '<?xml version=': 0, '>': 3, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4223378', 'n_tokens_mistral': 4496, 'n_tokens_neox': 4061, 'n_words': 721}