<commit_before>package org.usfirst.frc.team4915.stronghold.subsystems;

import org.usfirst.frc.team4915.stronghold.Robot;
import org.usfirst.frc.team4915.stronghold.RobotMap;
import org.usfirst.frc.team4915.stronghold.commands.IntakeLauncher.Aimer.AimLauncherCommand;
import org.usfirst.frc.team4915.stronghold.vision.robot.VisionState;

import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.CANTalon.TalonControlMode;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class IntakeLauncher extends Subsystem {

    // Ranges -1 to 1, negative values are reverse direction
    // Negative values indicate a wheel spinning outwards and positive values
    // indicate a wheel spinning inwards.
    private final double INTAKE_SPEED = -.60;
    private final double LAUNCH_SPEED_HIGH = 1.0;
    private final double LAUNCH_SPEED_LOW = 0.6; // TODO
    private final double ZERO_SPEED = 0.0;
    private final double AIM_DEGREES_SLOP = 2; // TODO: tune this number

    private final double LAUNCHER_MAX_HEIGHT_DEGREES = 45.0; // in degrees from
                                                             // horizontal
    private final double LAUNCHER_MIN_HEIGHT_DEGREES = -11.0; // in degrees from
                                                              // horizontal
    private final double SERVO_LEFT_LAUNCH_POSITION = .45; // in servo units
    private final double SERVO_RIGHT_LAUNCH_POSITION = .65; // in servo units
    private final double SERVO_LEFT_NEUTRAL_POSITION = .75; // in servo units
    private final double SERVO_RIGHT_NEUTRAL_POSITION = .4; // in servo units

    private double launcherMaxHeightTicks = 811.0; 
    private double launcherMinHeightTicks = 557.0; 
    
    private double launcherTravelHeightRatio = 0.175;
    private double launcherNeutralHeightRatio = 0.739;
    private double launcherMinLaunchHeightRatio = 0.2; // TODO
    
    private final double LAUNCHER_HIGH_GOAL_THRESHOLD = .85;
    private final double MAX_POTENTIOMETER_ERROR = 20;
    private final double APPROXIMATE_DANGER = 50;
    private final double JOYSTICK_SCALE = 50.0; // TODO
    private final double MIN_JOYSTICK_MOTION = 0.1;
    private final double NO_VISION_TARGET = -1000;
    private final int POTENTIOMETER_NEGATIVITY = -1;

    private boolean isJoystickIdle = false;
    private double setPoint; // in potentiometer ticks
    private double visionTarget = NO_VISION_TARGET;

    // left and right are determined when standing behind the robot
    // These motors control flywheels that collect and shoot the ball
    private CANTalon intakeLeftMotor = RobotMap.intakeLeftMotor; // ID 15
    private CANTalon intakeRightMotor = RobotMap.intakeRightMotor; // ID 14

    // This motor adjusts the angle of the launcher for shooting
    public CANTalon aimMotor = RobotMap.aimMotor; // ID 16

    // limitswitch in the back of the basket that tells the robot when the
    // boulder is secure
    public DigitalInput boulderSwitch = RobotMap.boulderSwitch;

    // These servos push the boulder into the launcher flywheels
    public Servo launcherServoLeft = RobotMap.launcherServoLeft; // port 0
    public Servo launcherServoRight = RobotMap.launcherServoRight; // port 1

    @Override
    protected void initDefaultCommand() {
        setDefaultCommand(new AimLauncherCommand());
        // setDefaultCommand(new BackUpJoystickControlCommand());
    }

    public IntakeLauncher() {
        readSetPoint();
    }

    public boolean IsAlive() {
        return this.aimMotor.isAlive();
    }

    // Sets the speed on the flywheels to suck in the boulder
    public void setSpeedIntake() {
        this.intakeLeftMotor.set(INTAKE_SPEED);
        this.intakeRightMotor.set(-INTAKE_SPEED);
    }

    // Sets the speed on the flywheels to launch the boulder
    public void setSpeedLaunchHigh() {
        this.intakeLeftMotor.set(LAUNCH_SPEED_HIGH);
        this.intakeRightMotor.set(-LAUNCH_SPEED_HIGH);
    }

    public void setSpeedLaunchLow() {
        this.intakeLeftMotor.set(LAUNCH_SPEED_LOW);
        this.intakeRightMotor.set(-LAUNCH_SPEED_LOW);
    }

    public void setDesiredWheelSpeed() {
        if (getPosition() > (launcherMinHeightTicks + ((launcherMaxHeightTicks - launcherMinHeightTicks) * LAUNCHER_HIGH_GOAL_THRESHOLD))) {
            setSpeedLaunchHigh();
        } else {
            setSpeedLaunchLow();
        }
    }

    public String getDesiredWheelSpeed() {
        if (getPosition() > (launcherMinHeightTicks + ((launcherMaxHeightTicks - launcherMinHeightTicks) * LAUNCHER_HIGH_GOAL_THRESHOLD))) {
            return "High Goal";
        } else {
            return "Low Goal";
        }
    }

    public void stopWheels() {
        this.intakeLeftMotor.set(ZERO_SPEED);
        this.intakeRightMotor.set(ZERO_SPEED);
    }

    public void activateLauncherServos() {
        this.launcherServoLeft.set(SERVO_LEFT_LAUNCH_POSITION);
        this.launcherServoRight.set(SERVO_RIGHT_LAUNCH_POSITION);
        System.out.println("Launched At: " + getPosition());
    }

    public void retractLauncherServos() {
        this.launcherServoLeft.set(SERVO_LEFT_NEUTRAL_POSITION);
        this.launcherServoRight.set(SERVO_RIGHT_NEUTRAL_POSITION);
    }

    private void readSetPoint() { // TODO rename
        setPoint = getPosition() * POTENTIOMETER_NEGATIVITY;
    }<commit_msg>mandatory commit for pulling<commit_after>package org.usfirst.frc.team4915.stronghold.subsystems;

import org.usfirst.frc.team4915.stronghold.Robot;
import org.usfirst.frc.team4915.stronghold.RobotMap;
import org.usfirst.frc.team4915.stronghold.commands.IntakeLauncher.Aimer.AimLauncherCommand;
import org.usfirst.frc.team4915.stronghold.vision.robot.VisionState;

import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.CANTalon.TalonControlMode;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class IntakeLauncher extends Subsystem {

    // Ranges -1 to 1, negative values are reverse direction
    // Negative values indicate a wheel spinning outwards and positive values
    // indicate a wheel spinning inwards.
    private final double INTAKE_SPEED = -.60;
    private final double LAUNCH_SPEED_HIGH = 1.0;
    private final double LAUNCH_SPEED_LOW = 0.6; // TODO
    private final double ZERO_SPEED = 0.0;
    private final double AIM_DEGREES_SLOP = 2; // TODO: tune this number

    private final double LAUNCHER_MAX_HEIGHT_DEGREES = 45.0; // in degrees from
                                                             // horizontal
    private final double LAUNCHER_MIN_HEIGHT_DEGREES = -11.0; // in degrees from
                                                              // horizontal
    private final double SERVO_LEFT_LAUNCH_POSITION = .45; // in servo units
    private final double SERVO_RIGHT_LAUNCH_POSITION = .65; // in servo units
    private final double SERVO_LEFT_NEUTRAL_POSITION = .75; // in servo units
    private final double SERVO_RIGHT_NEUTRAL_POSITION = .4; // in servo units

    private double launcherMaxHeightTicks = 811.0; 
    private double launcherMinHeightTicks = 557.0; 
    
    private double launcherTravelHeightRatio = 0.175;
    private double launcherNeutralHeightRatio = 1.0;
    private double launcherMinLaunchHeightRatio = 0.2; // TODO
    
    private final double LAUNCHER_HIGH_GOAL_THRESHOLD = .85;
    private final double MAX_POTENTIOMETER_ERROR = 20;
    private final double APPROXIMATE_DANGER = 50;
    private final double JOYSTICK_SCALE = 50.0; // TODO
    private final double MIN_JOYSTICK_MOTION = 0.1;
    private final double NO_VISION_TARGET = -1000;
    private final int POTENTIOMETER_NEGATIVITY = -1;

    private boolean isJoystickIdle = false;
    private double setPoint; // in potentiometer ticks
    private double visionTarget = NO_VISION_TARGET;

    // left and right are determined when standing behind the robot
    // These motors control flywheels that collect and shoot the ball
    private CANTalon intakeLeftMotor = RobotMap.intakeLeftMotor; // ID 15
    private CANTalon intakeRightMotor = RobotMap.intakeRightMotor; // ID 14

    // This motor adjusts the angle of the launcher for shooting
    public CANTalon aimMotor = RobotMap.aimMotor; // ID 16

    // limitswitch in the back of the basket that tells the robot when the
    // boulder is secure
    public DigitalInput boulderSwitch = RobotMap.boulderSwitch;

    // These servos push the boulder into the launcher flywheels
    public Servo launcherServoLeft = RobotMap.launcherServoLeft; // port 0
    public Servo launcherServoRight = RobotMap.launcherServoRight; // port 1

    @Override
    protected void initDefaultCommand() {
        setDefaultCommand(new AimLauncherCommand());
        // setDefaultCommand(new BackUpJoystickControlCommand());
    }

    public IntakeLauncher() {
        readSetPoint();
    }

    public boolean IsAlive() {
        return this.aimMotor.isAlive();
    }

    // Sets the speed on the flywheels to suck in the boulder
    public void setSpeedIntake() {
        this.intakeLeftMotor.set(INTAKE_SPEED);
        this.intakeRightMotor.set(-INTAKE_SPEED);
    }

    // Sets the speed on the flywheels to launch the boulder
    public void setSpeedLaunchHigh() {
        this.intakeLeftMotor.set(LAUNCH_SPEED_HIGH);
        this.intakeRightMotor.set(-LAUNCH_SPEED_HIGH);
    }

    public void setSpeedLaunchLow() {
        this.intakeLeftMotor.set(LAUNCH_SPEED_LOW);
        this.intakeRightMotor.set(-LAUNCH_SPEED_LOW);
    }

    public void setDesiredWheelSpeed() {
        setSpeedLaunchLow();
        //        if (getPosition() > (launcherMinHeightTicks + ((launcherMaxHeightTicks - launcherMinHeightTicks) * LAUNCHER_HIGH_GOAL_THRESHOLD))) {
//            setSpeedLaunchHigh();
//        } else {
//            setSpeedLaunchLow();
//        }
    }

    public String getDesiredWheelSpeed() {
        if (getPosition() > (launcherMinHeightTicks + ((launcherMaxHeightTicks - launcherMinHeightTicks) * LAUNCHER_HIGH_GOAL_THRESHOLD))) {
            return "High Goal";
        } else {
            return "Low Goal";
        }
    }

    public void stopWheels() {
        this.intakeLeftMotor.set(ZERO_SPEED);
        this.intakeRightMotor.set(ZERO_SPEED);
    }

    public void activateLauncherServos() {
        this.launcherServoLeft.set(SERVO_LEFT_LAUNCH_POSITION);
        this.launcherServoRight.set(SERVO_RIGHT_LAUNCH_POSITION);
        System.out.println("Launched At: " + getPosition());
    }

    public void retractLauncherServos() {
        this.launcherServoLeft.set(SERVO_LEFT_NEUTRAL_POSITION);
        this.launcherServoRight.set(SERVO_RIGHT_NEUTRAL_POSITION);
    }

    private void readSetPoint() { // TODO rename
        setPoint = getPosition() * POTENTIOMETER_NEGATIVITY;
    }
{'fraction_non_alphanumeric': 0.07813342923934544, 'fraction_numerical': 0.015285020679733861, 'mean_word_length': 3.0403196512895025, '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': 8, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1464888', 'n_tokens_mistral': 3714, 'n_tokens_neox': 3467, 'n_words': 802}