<commit_before>package org.usfirst.frc4915.MecanumDrive.subsystems;

import org.usfirst.frc4915.MecanumDrive.Robot;
import org.usfirst.frc4915.MecanumDrive.RobotMap;
import org.usfirst.frc4915.MecanumDrive.commands.elevator.ElevatorFineTune;
import org.usfirst.frc4915.debuggersystem.CustomDebugger.LoggerNames;

import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.command.Subsystem;

public class Elevator extends Subsystem {

	// These positions describe the number of totes you stacking on top of.
	// If you need to stack on top of 3 totes, use position 3.
	// If you need to stack on the ground, use position 0.

	// Not in inches. Between minimumPotentiometerValue and
	// maximumPotentiometerValue.
	public static double setPoint;

	// POTENTIOMTERS : fwd --> top, rev --> bottom

	// Set by ElevatorMax/MinHeightCalibrate commands
	public static double minimumPotentiometerValue = 0;
	public static double maximumPotentiometerValue = 1023;
	public static double slackMinimum = 0;
	
	public static final double APPROXIMATE_OFFSET = 410;
	public static boolean needToApproximate = true;
	
	public static boolean didSaveTopValue = false;
	public static boolean didSaveBottomValue = false;
	
	public static boolean SAFETY = true;
	
	public static final double RANGE_OF_MOTION = 53; // The elevator can go a
														// distance between 53
														// inches

	public static final double CHASIS_HEIGHT = 5; // These two measurements are
													// in inches
	public static final double HEIGHT_OF_TOTE = 12;

	private static final double JOYSTICK_SCALE = -5; // TODO Decide scale for
														// joystick movement
														// position change
	
	private static double previousJoystickY = 0;
	
	public CANTalon winch = RobotMap.elevatorWinchMotor;
	public DigitalInput bottomLimitSwitch = RobotMap.bottomLimitSwitch;
	
	/**
	 * Initializes the default command (WPI java default method) Called on
	 * initialization of the subsystem
	 */
	public void initDefaultCommand() {
		setDefaultCommand(new ElevatorFineTune());
		// Set the default command for a subsystem here.
		// setDefaultCommand(new MySpecialCommand());
	}

	/**
	 * Moves the elevator at a speed given by the joystick (y axis).
	 * 
	 * @param joystick Forward on joystick is up, backward is down
	 */
	public void moveWithJoystick(Joystick joystick) {
		double joystickY = joystick.getAxis(Joystick.AxisType.kY);
		Robot.debugger.logError(LoggerNames.ELEVATOR, "Elevator joystick " + joystickY);
		if (Math.abs(joystickY) <= .2) {
			Robot.debugger.logError(LoggerNames.ELEVATOR, "Joystick value too small");
			moveElevator(0);
		} else if (shouldStopElevator(joystickY)){
			setPoint = getPosition();
			moveElevator(0);
		} else {
			moveElevator(joystickY * JOYSTICK_SCALE);
		}
		previousJoystickY = joystickY;
	}
	
	/**
	 * If you push or pull the joystick from positive to negative/0, it will stop the elevator.
	 * Probably won't work because it is hard to move the joystick in the span of 20 ms.
	 * @param joystickY current joystickY value.
	 * @return true if the change in joystick value is more than 1 (so from + to -, or from - to +)
	 */
	private boolean shouldStopElevator(double joystickY) {
		return (Math.abs(joystickY - previousJoystickY) >= 1);
	}

	/**
	 * Changes height based on the input
	 * 
	 * @param heightChange
	 *            + goes up, - goes down
	 */
	public void moveElevator(double heightChange) {
		setPoint += heightChange;
		moveToHeight();
	}

	/**
	 * Moves the elevator to the height variable Ensures that height is in range
	 * first
	 */
	public void moveToHeight() {
		if (SAFETY) {
			keepHeightInRange();
		}
		winch.set(setPoint);
	}
	
	/**
	 * Sets the height to where it currently is so that the elevator should not go up or down.
	 */
	public void setHieghtToCurrentPosition() {
		setPoint = getPosition();
	}

	/**
	 * Stops the winch from winding. This may still have the elevator fall under
	 * it's own weight.
	 */
	public void stopElevator() {
		winch.disableControl();
		Robot.debugger.logError(LoggerNames.ELEVATOR, "Winch has stopped");
	}

	/**
	 * @return the position of the elevator in inches (between 0 and 54)
	 */
	public double getPositionInches() {
		double position = (getPosition() - minimumPotentiometerValue) 
						* (RANGE_OF_MOTION / (maximumPotentiometerValue - minimumPotentiometerValue));
		Robot.debugger.logError(LoggerNames.ELEVATOR, "The elevator is at position " + position + " (inches)");
		return position;
	}

	/**
	 * @return the read value from the potentiometer (between 0 and 1023)
	 */
	public double getPosition() {
		Robot.debugger.logError(LoggerNames.ELEVATOR, "The potentiometer reads " + winch.getPosition());
		return winch.getPosition();
	}

	/**
	 * Converts from a position between zero totes to six totes to inches.
	 * If you need to stack on top of 3 totes, use position 3.
	 * If you need to stack on the ground, use position 0.
	 * 
	 * @param positionNumber
	 *            the number of totes you are stacking on top of.
	 */
	public void setHeightToPosition(double positionNumber) {

		// find the range between the min and max Potentiometer values, divide by 54 to get
		// the change in value per inch and multiply by the number of inches that the totes are stacked
		setPoint = minimumPotentiometerValue + ((maximumPotentiometerValue - minimumPotentiometerValue) 
											 * HEIGHT_OF_TOTE * positionNumber / RANGE_OF_MOTION);
		Robot.debugger.logError(LoggerNames.ELEVATOR, "Elevator's height is " + setPoint);
	}
	
	/**
	 * 
	 * @return Level of Elevator in number of totes
	 */
	public double getElevatorLevel() {
		return ((RANGE_OF_MOTION * (winch.getPosition())) / (maximumPotentiometerValue - minimumPotentiometerValue)) / 12;
	}

	/**
	 * 
	 * @return if the elevator is at it's max height, return
	 */<commit_msg>Added getPositionNumber to elevator, removed getElevatorNumber because it doesn't work.<commit_after>package org.usfirst.frc4915.MecanumDrive.subsystems;

import org.usfirst.frc4915.MecanumDrive.Robot;
import org.usfirst.frc4915.MecanumDrive.RobotMap;
import org.usfirst.frc4915.MecanumDrive.commands.elevator.ElevatorFineTune;
import org.usfirst.frc4915.debuggersystem.CustomDebugger.LoggerNames;

import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.command.Subsystem;

public class Elevator extends Subsystem {

	// These positions describe the number of totes you stacking on top of.
	// If you need to stack on top of 3 totes, use position 3.
	// If you need to stack on the ground, use position 0.

	// Not in inches. Between minimumPotentiometerValue and
	// maximumPotentiometerValue.
	public static double setPoint;

	// POTENTIOMTERS : fwd --> top, rev --> bottom

	// Set by ElevatorMax/MinHeightCalibrate commands
	public static double minimumPotentiometerValue = 0;
	public static double maximumPotentiometerValue = 1023;
	public static double slackMinimum = 0;
	
	public static final double APPROXIMATE_OFFSET = 410;
	public static boolean needToApproximate = true;
	
	public static boolean didSaveTopValue = false;
	public static boolean didSaveBottomValue = false;
	
	public static boolean SAFETY = true;
	
	public static final double RANGE_OF_MOTION = 53; // The elevator can go a
														// distance between 53
														// inches

	public static final double CHASIS_HEIGHT = 5; // These two measurements are
													// in inches
	public static final double HEIGHT_OF_TOTE = 12;

	private static final double JOYSTICK_SCALE = -5; // TODO Decide scale for
														// joystick movement
														// position change
	
	private static double previousJoystickY = 0;
	
	public CANTalon winch = RobotMap.elevatorWinchMotor;
	public DigitalInput bottomLimitSwitch = RobotMap.bottomLimitSwitch;
	
	/**
	 * Initializes the default command (WPI java default method) Called on
	 * initialization of the subsystem
	 */
	public void initDefaultCommand() {
		setDefaultCommand(new ElevatorFineTune());
		// Set the default command for a subsystem here.
		// setDefaultCommand(new MySpecialCommand());
	}

	/**
	 * Moves the elevator at a speed given by the joystick (y axis).
	 * 
	 * @param joystick Forward on joystick is up, backward is down
	 */
	public void moveWithJoystick(Joystick joystick) {
		double joystickY = joystick.getAxis(Joystick.AxisType.kY);
		Robot.debugger.logError(LoggerNames.ELEVATOR, "Elevator joystick " + joystickY);
		if (Math.abs(joystickY) <= .2) {
			Robot.debugger.logError(LoggerNames.ELEVATOR, "Joystick value too small");
			moveElevator(0);
		} else if (shouldStopElevator(joystickY)){
			setPoint = getPosition();
			moveElevator(0);
		} else {
			moveElevator(joystickY * JOYSTICK_SCALE);
		}
		previousJoystickY = joystickY;
	}
	
	/**
	 * If you push or pull the joystick from positive to negative/0, it will stop the elevator.
	 * Probably won't work because it is hard to move the joystick in the span of 20 ms.
	 * @param joystickY current joystickY value.
	 * @return true if the change in joystick value is more than 1 (so from + to -, or from - to +)
	 */
	private boolean shouldStopElevator(double joystickY) {
		return (Math.abs(joystickY - previousJoystickY) >= 1);
	}

	/**
	 * Changes height based on the input
	 * 
	 * @param heightChange
	 *            + goes up, - goes down
	 */
	public void moveElevator(double heightChange) {
		setPoint += heightChange;
		moveToHeight();
	}

	/**
	 * Moves the elevator to the height variable Ensures that height is in range
	 * first
	 */
	public void moveToHeight() {
		if (SAFETY) {
			keepHeightInRange();
		}
		winch.set(setPoint);
	}
	
	/**
	 * Sets the height to where it currently is so that the elevator should not go up or down.
	 */
	public void setHieghtToCurrentPosition() {
		setPoint = getPosition();
	}

	/**
	 * Stops the winch from winding. This may still have the elevator fall under
	 * it's own weight.
	 */
	public void stopElevator() {
		winch.disableControl();
		Robot.debugger.logError(LoggerNames.ELEVATOR, "Winch has stopped");
	}

	/**
	 * @return the position of the elevator in inches (between 0 and 54)
	 */
	public double getPositionInches() {
		double position = (getPosition() - minimumPotentiometerValue) 
						* (RANGE_OF_MOTION / (maximumPotentiometerValue - minimumPotentiometerValue));
		Robot.debugger.logError(LoggerNames.ELEVATOR, "The elevator is at position " + position + " (inches)");
		return position;
	}
	
	/**
	 * @return the position number of the elevator -- how many totes 
	 * could be below it. Between 0 and 5.
	 */
	public double getPositionNumber() {
		double positionNumber = getPositionInches() / HEIGHT_OF_TOTE;
		return positionNumber;
	}

	/**
	 * @return the read value from the potentiometer (between 0 and 1023)
	 */
	public double getPosition() {
		Robot.debugger.logError(LoggerNames.ELEVATOR, "The potentiometer reads " + winch.getPosition());
		return winch.getPosition();
	}

	/**
	 * Converts from a position between zero totes to six totes to inches.
	 * If you need to stack on top of 3 totes, use position 3.
	 * If you need to stack on the ground, use position 0.
	 * 
	 * @param positionNumber
	 *            the number of totes you are stacking on top of.
	 */
	public void setHeightToPosition(double positionNumber) {

		// find the range between the min and max Potentiometer values, divide by 54 to get
		// the change in value per inch and multiply by the number of inches that the totes are stacked
		setPoint = minimumPotentiometerValue + ((maximumPotentiometerValue - minimumPotentiometerValue) 
											 * HEIGHT_OF_TOTE * positionNumber / RANGE_OF_MOTION);
		Robot.debugger.logError(LoggerNames.ELEVATOR, "Elevator's height is " + setPoint);
	}

	/**
	 * 
	 * @return if the elevator is at it's max height, return
	 */
{'fraction_non_alphanumeric': 0.08711297071129707, 'fraction_numerical': 0.010711297071129708, 'mean_word_length': 3.591240875912409, 'pattern_counts': {'":': 0, '<': 5, '<?xml version=': 0, '>': 9, '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': '4157867', 'n_tokens_mistral': 4239, 'n_tokens_neox': 3595, 'n_words': 1199}