<commit_before>package frc.team4215.stronghold;

import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.Timer;

/**
 * The class for Autonomous.
 *
 * @author James
 */
public class Autonomous {
    
    private static Thread threadPing;
    private static double position;
    static Timer time = new Timer();
    /**
     * Seconds of Autonomous period
     */
    private static final double AUTOTIME = 15;
    /**
     * Sample rate, times/second.
     */
    private static final double SAMPLINGRATE = 20;
    private javax.management.timer.Timer timer;

    // private Victor armMotor, intake;

    private DriveTrain dT;

    private Arm arm;
    
    private Intake intake;

    private Interface choiceAuto;

    public Autonomous(DriveTrain dT_) throws RobotException {
        dT = dT_;
        arm = new Arm();
        intake = new Intake();
    }

    public void chooseAuto(int num) {
        if (num == 1) choiceAuto = () -> autoLowBar();
        else if (num == 2)
            choiceAuto = () -> autoSpyBotLowGoal();
        else if (num == 3)
            choiceAuto = () -> autoChevalDeFrise();
        else if (num == 4)
            choiceAuto = () -> autoPortcullis();
        else choiceAuto = null;
    }

    public void autoChoice() throws RobotException {
        if (null != choiceAuto)
            throw new RobotException("There is not a method chosen.");
        choiceAuto.runAuto();
    }

    /* working variables */
    private double lastTime;
    private double Input, Output, Setpoint;
    private double errSum, lastErr;
    private double kp, ki, kd;

    /**
     * PID controller
     *
     * @author <NAME>
     */
/*    public double errorCompute() {
    	// How long since we last calculated
    	double now = time.get();
    	double timeChange = now - lastTime;

    	// Compute all the working error variables
    	double error = Setpoint - Input;
    	errSum += (error * timeChange);
    	double dErr = (error - lastErr) / timeChange;

    	// Compute PID Output
    	Output = kp * error + ki * errSum
    			+ kd * dErr;

    	// Remember some variables for next time
    	lastErr = error;
    	lastTime = now;

    	return Output;
    }

    void SetTunings(double Kp, double Ki, double Kd) {
        kp = Kp;
        ki = Ki;
        kd = Kd;
    }
    // DO NOT DELETE THIS CODE EVER!!!!!
    */

    /** 
     * PID controller implementation for accelerometer
     * 
     * Waweru and I have decided that the derivative part for the
     * controller is unnecessary. Derivative function taken out.
     * 
     * @author Joey
     */
    public double accelerometerPID(double accelerometerKp, double accelerometerKi){
    	// Time since last calculation
    	double now = time.get();
        double timeChange = now - lastTime;
        
        //Calculate error variables
        double error = Setpoint - Input;
        errSum += (error * timeChange);
        
        //Sum errors
        double accelerometerOutput = accelerometerKp * error + accelerometerKi * errSum;
        
        //Reset time variable
        lastTime = now;
        
        return accelerometerOutput;
    }
    
    /**
     * PID controller implementation for gyroscope
     * 
     * @param gyroKp
     * @param gyroKi
     * @author Joey
     */
    public double gyroPID(double gyroKp, double gyroKi){
    	// Time since last calculation
    	double now = time.get();
        double timeChange = now - lastTime;
        
        //Calculate error variables
        double error = Setpoint - Input;
        errSum += (error * timeChange);
        
        //Sum errors
        double gyroOutput = gyroKp * error + gyroKi * errSum;
        
        //Reset time variable
        lastTime = now;
        
        return gyroOutput;
    }
    
    /**
     * Method called to set the Setpoint so the PID controller has the
     * capability to calculate errors and correct them.
     *
     * @param defSetpoint
     *            double value
     * @author <NAME>
     */
     public static void setSetpoint(double defSetpoint){
    	double Setpoint = defSetpoint;
    	}

    

    /**
     * Timer method integrating the Timer class from wpilibj. USE THIS
     * TIMER UNIVERSALLY!!!!!
     *
     * @author <NAME>
     */
    public static void startTimer() {
        time.start();
    }


    /**
     * All constants.
     *
     * @author James
     */
    private static final class Constant {

        /**
         * Constant shared.
         *
         * @author James
         */
        public static final class Shared {

            static final double armMoveMaxTime = 2d;

            static final double armDown = -1, armUp = 1, armStop = 0;

            public static final double intakeDelay = 1d;
        }

        /**
         * Constant for autoLowBar.
         *
         * @author James
         */
        private static final class ConstLowBar {

            public static final double driveThroughDelay = 5d;
        }

        /**
         * Constant for autoSpyBotLowGoal.
         *
         * @author James
         */
        private static final class ConstSpyBotLowGoal {

            public static final double driveToDelay = 5d;
        }

        /**
         * Constant for autoChevalDeFrise.
         *
         * @author James
         */
        private static final class ConstChevalDeFrise {

            public static final double driveToDelay = 5d;
            public static final double driveThroughDelay = 5d;
        }

        /**
         * Constant for autoPortcullis.
         *
         * @author James
         */
        public static final class ConstPortcullis {

            public static final double driveDelay = 5d;
            public static final double driveThroughDelay = 5d;
        }
    }

    /**
     * The interface for programs outside to run the chosen autonomous
     * function.
     *
     * @author James
     */
    public interface Interface {

        public void runAuto();
    }

    /**
     * to lower arm. Need more info.
     *
     * @author James
     */
    private void armLowerBottom() {
        arm.set(Constant.Shared.armDown);
        Autonomous.delay(Constant.Shared.armMoveMaxTime);
        arm.set(Constant.Shared.armStop);
    }

    /**
     * to delay for some time. Need more info.
     *
     * @author James
     * @param delayTime
     *            delay time in seconds
     */
    private static void delay(double delayTime) {
        // Just realized there is a delay thing in Timer. But I'd just
        // stick to using Autonomous.delay.
        edu.wpi.first.wpilibj.Timer.delay(delayTime);
    }

    /**
     * to lift arm. Need more info
     *
     * @author James
     */
    private void armLifterTop() {
        arm.set(Constant.Shared.armUp);
        Autonomous.delay(Constant.Shared.armMoveMaxTime);
        arm.set(Constant.Shared.armStop);
    }

    /**
     * To drive straight some distance.
     *
     * @author James
     * @param driveDistance
     *            Meters of driving
     * @param PLACEHOLDER
     *            This is just a placeholder and does not do anything.
     *            You can just use empty string "" for 
     */
    private void driveStraight(double driveDistance,
            Object PLACEHOLDER) {
        PLACEHOLDER = "";
    }

    /**
     * Place Holder. To drive straight. Need more info.
     *
     * @author James
     * @param driveTime
     *            Seconds of driving
     */
    private void driveStraight(double driveTime) {
        // command starts
        dT.drive(Const.Motor.Run.Forward);
        Autonomous.delay(driveTime);
        dT.drive(Const.Motor.Run.Stop);
    }
    
    /**
     * throw ball out. Yet tested.
     *
     * @author James
     */
    private void throwBall() {
        intake.set(Const.Motor.Run.Forward);
        Autonomous.delay(Constant.Shared.intakeDelay);
        intake.set(Const.Motor.Run.Stop);
    }

    /**
     * Autonomous function No.1
     *
     * @author James
     */
    public void autoLowBar() {
        armLowerBottom();
        driveStraight(Constant.ConstLowBar.driveThroughDelay);
    }

    /**
     * Autonomous function No.2
     *
     * @author James
     */
    public void autoSpyBotLowGoal() {
        armLowerBottom();
        driveStraight(Constant.ConstSpyBotLowGoal.driveToDelay);
        throwBall();
    }

    /**
     * Autonomous function No.3
     *
     * @author James
     */
    public void autoChevalDeFrise() {
        driveStraight(Constant.ConstChevalDeFrise.driveToDelay);
        armLowerBottom();
        driveStraight(
                Constant.ConstChevalDeFrise.driveThroughDelay);
    }

    /**
     * Autonomous function No.4
     *
     * @author James
     */
    public void autoPortcullis() {
        armLowerBottom();
        driveStraight(Constant.ConstPortcullis.driveDelay);
        armLifterTop();
        driveStraight(
                Constant.ConstPortcullis.driveThroughDelay);
    }

    /**
     * Should be equivalent to a method called getAccel of another
     * class I2CAccelerometer which isn't here yet.
     *
     * @author James
     * @return Accelerations, double[] with length of 3
     */
    private static double[] I2CAccelerometer_getAccel() {
        double[] accel = new double[3];
        return accel; // placeholder
    }

    /**
     * Calculates distance traveled based on information from the
     * accelerometer.
     *
     * @author Joey
     * @return
     */
    private static void I2CDistanceTraveled() {

        double time1 = time.get();
        for (int count = 0; count < (Autonomous.AUTOTIME
                * Autonomous.SAMPLINGRATE); count++) {
        	delay(1 / Autonomous.SAMPLINGRATE);
            double time2 = time.get();
            double[] acceleration = I2CAccelerometer_getAccel();

            position += .5 * acceleration[0] * (time2 - time1);
        }
    }
    
    public static void pingerStart() {
        Runnable pinger = () -> {
            while (true)
                Autonomous.I2CAccelerometer_getAccel();
        };

        Autonomous.threadPing = new Thread(pinger);
        Autonomous.threadPing.start();<commit_msg>Rename variable for clarification<commit_after>package frc.team4215.stronghold;

import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.Timer;

/**
 * The class for Autonomous.
 *
 * @author James
 */
public class Autonomous {
    
    private static Thread threadPing;
    private static double distanceTraveled;
    static Timer time = new Timer();
    /**
     * Seconds of Autonomous period
     */
    private static final double AUTOTIME = 15;
    /**
     * Sample rate, times/second.
     */
    private static final double SAMPLINGRATE = 20;
    private javax.management.timer.Timer timer;

    // private Victor armMotor, intake;

    private DriveTrain dT;

    private Arm arm;
    
    private Intake intake;

    private Interface choiceAuto;

    public Autonomous(DriveTrain dT_) throws RobotException {
        dT = dT_;
        arm = new Arm();
        intake = new Intake();
    }

    public void chooseAuto(int num) {
        if (num == 1) choiceAuto = () -> autoLowBar();
        else if (num == 2)
            choiceAuto = () -> autoSpyBotLowGoal();
        else if (num == 3)
            choiceAuto = () -> autoChevalDeFrise();
        else if (num == 4)
            choiceAuto = () -> autoPortcullis();
        else choiceAuto = null;
    }

    public void autoChoice() throws RobotException {
        if (null != choiceAuto)
            throw new RobotException("There is not a method chosen.");
        choiceAuto.runAuto();
    }

    /* working variables */
    private double lastTime;
    private double Input, Output, Setpoint;
    private double errSum, lastErr;
    private double kp, ki, kd;

    /**
     * PID controller
     *
     * @author <NAME>
     */
/*    public double errorCompute() {
    	// How long since we last calculated
    	double now = time.get();
    	double timeChange = now - lastTime;

    	// Compute all the working error variables
    	double error = Setpoint - Input;
    	errSum += (error * timeChange);
    	double dErr = (error - lastErr) / timeChange;

    	// Compute PID Output
    	Output = kp * error + ki * errSum
    			+ kd * dErr;

    	// Remember some variables for next time
    	lastErr = error;
    	lastTime = now;

    	return Output;
    }

    void SetTunings(double Kp, double Ki, double Kd) {
        kp = Kp;
        ki = Ki;
        kd = Kd;
    }
    // DO NOT DELETE THIS CODE EVER!!!!!
    */

    /** 
     * PID controller implementation for accelerometer
     * 
     * Waweru and I have decided that the derivative part for the
     * controller is unnecessary. Derivative function taken out.
     * 
     * @author Joey
     */
    public double accelerometerPID(double accelerometerKp, double accelerometerKi){
    	// Time since last calculation
    	double now = time.get();
        double timeChange = now - lastTime;
        
        //Calculate error variables
        double error = Setpoint - Input;
        errSum += (error * timeChange);
        
        //Sum errors
        double accelerometerOutput = accelerometerKp * error + accelerometerKi * errSum;
        
        //Reset time variable
        lastTime = now;
        
        return accelerometerOutput;
    }
    
    /**
     * PID controller implementation for gyroscope
     * 
     * @param gyroKp
     * @param gyroKi
     * @author Joey
     */
    public double gyroPID(double gyroKp, double gyroKi){
    	// Time since last calculation
    	double now = time.get();
        double timeChange = now - lastTime;
        
        //Calculate error variables
        double error = Setpoint - Input;
        errSum += (error * timeChange);
        
        //Sum errors
        double gyroOutput = gyroKp * error + gyroKi * errSum;
        
        //Reset time variable
        lastTime = now;
        
        return gyroOutput;
    }
    
    /**
     * Method called to set the Setpoint so the PID controller has the
     * capability to calculate errors and correct them.
     *
     * @param defSetpoint
     *            double value
     * @author <NAME>
     */
     public static void setSetpoint(double defSetpoint){
    	double Setpoint = defSetpoint;
    	}

    

    /**
     * Timer method integrating the Timer class from wpilibj. USE THIS
     * TIMER UNIVERSALLY!!!!!
     *
     * @author <NAME>
     */
    public static void startTimer() {
        time.start();
    }


    /**
     * All constants.
     *
     * @author James
     */
    private static final class Constant {

        /**
         * Constant shared.
         *
         * @author James
         */
        public static final class Shared {

            static final double armMoveMaxTime = 2d;

            static final double armDown = -1, armUp = 1, armStop = 0;

            public static final double intakeDelay = 1d;
        }

        /**
         * Constant for autoLowBar.
         *
         * @author James
         */
        private static final class ConstLowBar {

            public static final double driveThroughDelay = 5d;
        }

        /**
         * Constant for autoSpyBotLowGoal.
         *
         * @author James
         */
        private static final class ConstSpyBotLowGoal {

            public static final double driveToDelay = 5d;
        }

        /**
         * Constant for autoChevalDeFrise.
         *
         * @author James
         */
        private static final class ConstChevalDeFrise {

            public static final double driveToDelay = 5d;
            public static final double driveThroughDelay = 5d;
        }

        /**
         * Constant for autoPortcullis.
         *
         * @author James
         */
        public static final class ConstPortcullis {

            public static final double driveDelay = 5d;
            public static final double driveThroughDelay = 5d;
        }
    }

    /**
     * The interface for programs outside to run the chosen autonomous
     * function.
     *
     * @author James
     */
    public interface Interface {

        public void runAuto();
    }

    /**
     * to lower arm. Need more info.
     *
     * @author James
     */
    private void armLowerBottom() {
        arm.set(Constant.Shared.armDown);
        Autonomous.delay(Constant.Shared.armMoveMaxTime);
        arm.set(Constant.Shared.armStop);
    }

    /**
     * to delay for some time. Need more info.
     *
     * @author James
     * @param delayTime
     *            delay time in seconds
     */
    private static void delay(double delayTime) {
        // Just realized there is a delay thing in Timer. But I'd just
        // stick to using Autonomous.delay.
        edu.wpi.first.wpilibj.Timer.delay(delayTime);
    }

    /**
     * to lift arm. Need more info
     *
     * @author James
     */
    private void armLifterTop() {
        arm.set(Constant.Shared.armUp);
        Autonomous.delay(Constant.Shared.armMoveMaxTime);
        arm.set(Constant.Shared.armStop);
    }

    /**
     * To drive straight some distance.
     *
     * @author James
     * @param driveDistance
     *            Meters of driving
     * @param PLACEHOLDER
     *            This is just a placeholder and does not do anything.
     *            You can just use empty string "" for 
     */
    private void driveStraight(double driveDistance,
            Object PLACEHOLDER) {
        PLACEHOLDER = "";
    }

    /**
     * Place Holder. To drive straight. Need more info.
     *
     * @author James
     * @param driveTime
     *            Seconds of driving
     */
    private void driveStraight(double driveTime) {
        // command starts
        dT.drive(Const.Motor.Run.Forward);
        Autonomous.delay(driveTime);
        dT.drive(Const.Motor.Run.Stop);
    }
    
    /**
     * throw ball out. Yet tested.
     *
     * @author James
     */
    private void throwBall() {
        intake.set(Const.Motor.Run.Forward);
        Autonomous.delay(Constant.Shared.intakeDelay);
        intake.set(Const.Motor.Run.Stop);
    }

    /**
     * Autonomous function No.1
     *
     * @author James
     */
    public void autoLowBar() {
        armLowerBottom();
        driveStraight(Constant.ConstLowBar.driveThroughDelay);
    }

    /**
     * Autonomous function No.2
     *
     * @author James
     */
    public void autoSpyBotLowGoal() {
        armLowerBottom();
        driveStraight(Constant.ConstSpyBotLowGoal.driveToDelay);
        throwBall();
    }

    /**
     * Autonomous function No.3
     *
     * @author James
     */
    public void autoChevalDeFrise() {
        driveStraight(Constant.ConstChevalDeFrise.driveToDelay);
        armLowerBottom();
        driveStraight(
                Constant.ConstChevalDeFrise.driveThroughDelay);
    }

    /**
     * Autonomous function No.4
     *
     * @author James
     */
    public void autoPortcullis() {
        armLowerBottom();
        driveStraight(Constant.ConstPortcullis.driveDelay);
        armLifterTop();
        driveStraight(
                Constant.ConstPortcullis.driveThroughDelay);
    }

    /**
     * Should be equivalent to a method called getAccel of another
     * class I2CAccelerometer which isn't here yet.
     *
     * @author James
     * @return Accelerations, double[] with length of 3
     */
    private static double[] I2CAccelerometer_getAccel() {
        double[] accel = new double[3];
        return accel; // placeholder
    }

    /**
     * Calculates distance traveled based on information from the
     * accelerometer.
     *
     * @author Joey
     * @return
     */
    private static void I2CDistanceTraveled() {

        double time1 = time.get();
        for (int count = 0; count < (Autonomous.AUTOTIME
                * Autonomous.SAMPLINGRATE); count++) {
        	delay(1 / Autonomous.SAMPLINGRATE);
            double time2 = time.get();
            double[] acceleration = I2CAccelerometer_getAccel();

            distanceTraveled += .5 * acceleration[0] * (time2 - time1);
        }
    }
    
    public static void pingerStart() {
        Runnable pinger = () -> {
            while (true)
                Autonomous.I2CAccelerometer_getAccel();
        };

        Autonomous.threadPing = new Thread(pinger);
        Autonomous.threadPing.start();
{'fraction_non_alphanumeric': 0.09137998436278343, 'fraction_numerical': 0.004104769351055512, 'mean_word_length': 1.7588298732812078, 'pattern_counts': {'":': 0, '<': 11, '<?xml version=': 0, '>': 19, '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': '131974', 'n_tokens_mistral': 6000, 'n_tokens_neox': 5664, 'n_words': 1612}