<commit_before>package org.texastorque.texastorque2015.input;

import edu.wpi.first.wpilibj.Timer;
import org.texastorque.texastorque2015.constants.Constants;
import org.texastorque.torquelib.util.GenericController;
import org.texastorque.torquelib.util.TorqueToggle;

public class DriverInput extends Input {

    GenericController driver;
    OperatorConsole operator;

    TorqueToggle tiltToggle;
    TorqueToggle armOpenToggle;

    private boolean wentToBottom;
    private double toteInTime;
    private boolean toteAvailable;

    public DriverInput() {
        driver = new GenericController(0, GenericController.TYPE_XBOX, 0.2);
        operator = new OperatorConsole(1);

        tiltToggle = new TorqueToggle();
        armOpenToggle = new TorqueToggle();
        
        wentToBottom = false;
        toteAvailable = false;
    }

    @Override
    public void run() {
        //Drivebase
        calcDrivebase();

        //Elevator
        elevatorOverride = operator.getElevatorOverrideSwitch();
        elevatorFFpOff = operator.getElevatorFFpOffSwitch();

        if (operator.getAutoStackButton()) {
            autoStack = true;
        }
        feederStack = operator.getFeederStackButton();

        //Calculate what all of the subsystems should do either independently or 
        //synchronized for complicated actions.
        if (elevatorOverride) {
            calcElevatorOverride();
            calcArms();
            calcIntake();
        } else if (autoStack) {
            armOpen = false;
            tiltUp = false;
            punchOut = false;

            if (elevatorPosition == Constants.FloorElevatorLevel1.getDouble() && feedback.isElevatorDone()) {
                elevatorPosition = Constants.FloorElevatorLevel2.getDouble();
                wentToBottom = true;
                intakeSpeed = 0.0;
                intakesIn = false;
            } else if (elevatorPosition == Constants.FloorElevatorLevel2.getDouble() && wentToBottom && feedback.isElevatorDone()) {
                autoStack = false;
                toteAvailable = false;
            } else {
                elevatorPosition = Constants.FloorElevatorLevel1.getDouble();
                intakeSpeed = 1.0;
                intakesIn = true;
            }
        } else if (feederStack) {
            double currentTime = Timer.getFPGATimestamp();

            elevatorPosition = Constants.FloorElevatorLevel2.getDouble();
            armOpen = false;
            punchOut = false;
            tiltUp = false;

            if (feedback.isToteInSluice() && !toteAvailable) {
                toteInTime = Timer.getFPGATimestamp();
                toteAvailable = true;
            } else if (toteAvailable) {
                if (currentTime - Constants.ToteSluiceWaitTime.getDouble() > toteInTime) {
                    intakeSpeed = 1.0;
                    if (currentTime - Constants.ToteSluiceWaitTime.getDouble() - Constants.TotePullBAckTime.getDouble() > toteInTime) {
                        autoStack = true;
                    }
                } else {
                    if (feedback.isElevatorDone()) {
                        intakeSpeed = -1.0;
                        intakesIn = true;
                    } else {
                        intakeSpeed = 0.0;
                        intakesIn = false;
                    }
                }
            }
        } else {
            calcElevator();
            calcArms();
            calcIntake();
        }
    }

//Drivebase
    private void calcDrivebase() {
        /**
         * Left stick controls translation, right stick controls rotation. Both
         * the forward and strafe wheels are utilized for rotation.
         */
        if (driver.getRightBumper()) { //Turn over front of robot
            leftSpeed = -1 * driver.getLeftYAxis() + driver.getRightXAxis() * 0.1;
            rightSpeed = -1 * driver.getLeftYAxis() - driver.getRightXAxis() * 0.1;
            frontStrafeSpeed = -1 * driver.getLeftXAxis() - driver.getRightXAxis() * 121 / 400;
            rearStrafeSpeed = -1 * driver.getLeftXAxis() + driver.getRightXAxis();
        } else if (driver.getLeftBumper()) { //Turn over back of robot
            leftSpeed = -1 * driver.getLeftYAxis() + driver.getRightXAxis() * 0.1;
            rightSpeed = -1 * driver.getLeftYAxis() - driver.getRightXAxis() * 0.1;
            frontStrafeSpeed = -1 * driver.getLeftXAxis() - driver.getRightXAxis();<commit_msg>Fixed and cleaned up automation<commit_after>package org.texastorque.texastorque2015.input;

import edu.wpi.first.wpilibj.Timer;
import org.texastorque.texastorque2015.constants.Constants;
import org.texastorque.torquelib.util.GenericController;
import org.texastorque.torquelib.util.TorqueToggle;

public class DriverInput extends Input {

    GenericController driver;
    OperatorConsole operator;

    TorqueToggle tiltToggle;
    TorqueToggle armOpenToggle;

    private boolean wentToBottom;
    private double toteInTime;
    private boolean toteAvailable;

    public DriverInput() {
        driver = new GenericController(0, GenericController.TYPE_XBOX, 0.2);
        operator = new OperatorConsole(1);

        tiltToggle = new TorqueToggle();
        armOpenToggle = new TorqueToggle();

        wentToBottom = false;
        toteAvailable = false;
    }

    @Override
    public void run() {
        //Drivebase
        calcDrivebase();

        //Elevator
        elevatorOverride = operator.getElevatorOverrideSwitch();
        elevatorFFpOff = operator.getElevatorFFpOffSwitch();

        if (operator.getAutoStackButton()) {
            autoStack = true;
        }
        feederStack = operator.getFeederStackButton();

        //Calculate what all of the subsystems should do either independently or 
        //synchronized for complicated actions.
        if (elevatorOverride) {
            calcElevatorOverride();
            calcArms();
            calcIntake();
        } else if (autoStack) {
            armOpen = false;
            tiltUp = false;
            punchOut = false;

            if (elevatorPosition == Constants.FloorElevatorLevel1.getDouble() && feedback.isElevatorDone()) {
                elevatorPosition = Constants.FloorElevatorLevel2.getDouble();
                wentToBottom = true;
                intakeSpeed = 0.0;
                intakesIn = false;
            } else if (elevatorPosition == Constants.FloorElevatorLevel2.getDouble() && wentToBottom && feedback.isElevatorDone()) {
                autoStack = false;
                toteAvailable = false;
                wentToBottom = false;
            } else {
                elevatorPosition = Constants.FloorElevatorLevel1.getDouble();
                intakeSpeed = 1.0;
                intakesIn = true;
            }
        } else if (feederStack) {
            double currentTime = Timer.getFPGATimestamp();

            elevatorPosition = Constants.FloorElevatorLevel2.getDouble();
            armOpen = false;
            punchOut = false;
            tiltUp = false;

            if (feedback.isToteInSluice() && !toteAvailable) {
                toteInTime = Timer.getFPGATimestamp();
                toteAvailable = true;
            } else if (toteAvailable) {
                if (currentTime - Constants.ToteSluiceWaitTime.getDouble() > toteInTime) {
                    if (currentTime - Constants.ToteSluiceWaitTime.getDouble() - Constants.TotePullBAckTime.getDouble() > toteInTime) {
                        autoStack = true;
                    } else {
                        intakeSpeed = 1.0;
                    }
                } else {
                    intakeSpeed = -1.0;
                    intakesIn = true;
                }
            }
        } else {
            calcElevator();
            calcArms();
            calcIntake();
        }
    }

    //Drivebase
    private void calcDrivebase() {
        /**
         * Left stick controls translation, right stick controls rotation. Both
         * the forward and strafe wheels are utilized for rotation.
         */
        if (driver.getRightBumper()) { //Turn over front of robot
            leftSpeed = -1 * driver.getLeftYAxis() + driver.getRightXAxis() * 0.1;
            rightSpeed = -1 * driver.getLeftYAxis() - driver.getRightXAxis() * 0.1;
            frontStrafeSpeed = -1 * driver.getLeftXAxis() - driver.getRightXAxis() * 121 / 400;
            rearStrafeSpeed = -1 * driver.getLeftXAxis() + driver.getRightXAxis();
        } else if (driver.getLeftBumper()) { //Turn over back of robot
            leftSpeed = -1 * driver.getLeftYAxis() + driver.getRightXAxis() * 0.1;
            rightSpeed = -1 * driver.getLeftYAxis() - driver.getRightXAxis() * 0.1;
            frontStrafeSpeed = -1 * driver.getLeftXAxis() - driver.getRightXAxis();
{'fraction_non_alphanumeric': 0.09267461669505962, 'fraction_numerical': 0.010675752413401476, 'mean_word_length': 1.8609486679662117, 'pattern_counts': {'":': 0, '<': 3, '<?xml version=': 0, '>': 7, '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': '1460279', 'n_tokens_mistral': 2503, 'n_tokens_neox': 2384, 'n_words': 501}