Elevator.java

package org.usfirst.frc.team236.robot.subsystems;

import org.usfirst.frc.team236.robot.PID;
import org.usfirst.frc.team236.robot.Robot;
import org.usfirst.frc.team236.robot.RobotMap;
import org.usfirst.frc.team236.robot.Updatable;
import org.usfirst.frc.team236.robot.Updater;
import org.usfirst.frc.team236.robot.commands.elevator.StopElevator;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 *
 */
public class Elevator extends Subsystem implements Updatable, PIDSource,
		PIDOutput {
	
	public static final int FLOOR = 0, PLATFORM = 1, STEP = 2;
	public static int toteHeight = 2;

	public static int level = FLOOR;
	private Encoder encoder; 
	private DigitalInput bottomLimit; // bottomLimit must be normally false
	private DigitalInput topLimit; // topLimit must be normally false
	private SpeedController leftMotor, rightMotor;
	private PID pid = new PID(this, this);
	private DigitalInput testLimit;

	// constructor
	public Elevator() {
		encoder = new Encoder(RobotMap.ElevatorMap.encoderDioA,
				RobotMap.ElevatorMap.encoderDioB);
		bottomLimit = new DigitalInput(RobotMap.ElevatorMap.bottomLimitDio);
		topLimit = new DigitalInput(RobotMap.ElevatorMap.topLimitDio);
		leftMotor = new Talon(RobotMap.ElevatorMap.leftPwm);
		rightMotor = new Talon(RobotMap.ElevatorMap.rightPwm);
		encoder.reset();
		testLimit = new DigitalInput(RobotMap.ElevatorMap.testLimitDio);
		//give the pid controller to the scheduler, but run in a separate thread
		Updater.getInstance().addElevatorUpdater(pid);
		//the pid controller will automatically call the appropriate methods to read sensor data
		//and set motor power in its own thread, which updates faster than the main robot thread
		//set the default gains
		
		pid.setGains(RobotMap.ElevatorMap.emptyPid);
		
		if(Robot.isElevatorDebug){
			SmartDashboard.putNumber("Elevator kP", 0);
			SmartDashboard.putNumber("Elevator kI", 0);
			SmartDashboard.putNumber("Elevator kD", 0);
			SmartDashboard.putNumber("Elevator Max I", 0);
			SmartDashboard.putNumber("Elevator Max Error", 0);
			SmartDashboard.putNumber("Elevator Max dError/dt", 0);
		}
	}
	
	public boolean onTarget(){
		return Math.abs(pid.setPoint - getHeight()) < RobotMap.ElevatorMap.heightTolerance;
	}
	
	//enable or disable the PID controller
	public void setPIDEnabled(boolean isEnabled){
		pid.enabled = isEnabled;
	}
	
	

	// return the current height of the elevator in inches
	public double getHeight() {
		return encoder.get() * RobotMap.ElevatorMap.distancePerPulse;
	}

	@Override
	public void update() {
		SmartDashboard.putBoolean("Elevator Limit", bottomLimit.get());
		SmartDashboard.putBoolean("INTAKE_LIMIT", !topLimit.get());
		SmartDashboard.putNumber("Elevator Height", getHeight());
		SmartDashboard.putNumber("Elevator Set Height", pid.setPoint);
		SmartDashboard.putBoolean("Test Limit", testLimit.get());
		if (!bottomLimit.get()) {
			encoder.reset();
		}
		
	}
	
	public boolean getLowerLimit(){
		return !bottomLimit.get();
	}

	public void setHeight(double targetHeight) {
		pid.setPoint = targetHeight;
	}
	
	//Computes how high elevator should be (set point), depending on 2 parameters:
	// 1. toteHeight (number of totes in stack, 1 through 6)   
	// 2. offset (varies depending on if you want to pick up from Floor, Scoring Platform, or Step)
	// These two parameters are linked to controller buttons in joystick map
	public static double findElevatorHeight(int toteHeight, int floorHeight){

		SmartDashboard.putNumber("Tote Stack Height", toteHeight);
		
		if(floorHeight == Elevator.FLOOR){
			SmartDashboard.putString("Stacking Offset", "Floor");
			return toteHeight * RobotMap.ElevatorMap.toteHeight;
		}else if(floorHeight == Elevator.PLATFORM){
			SmartDashboard.putString("Stacking Offset", "Platform");
			return RobotMap.ElevatorMap.scoringPlatformOffset +
					(toteHeight * RobotMap.ElevatorMap.toteHeight);
		}else{
			SmartDashboard.putString("Stacking Offset", "Step");
			return RobotMap.ElevatorMap.stepOffset + 
					(toteHeight * RobotMap.ElevatorMap.toteHeight);
		}
	}
	
	
	
	//always use setPower to set motor power so you don't try to move below min or above max elevator travel
	@SuppressWarnings("unused")
    private void setPower(double motorSpeed) {
		if(Robot.oi.wantSlowElevator()){
			//coerce elevator values for crawl mode
			motorSpeed = coerce(motorSpeed, -.25, .4);
		}
		if (!bottomLimit.get() && motorSpeed < 0) {
			leftMotor.set(0);
			rightMotor.set(0);
		} else if (false) {
			leftMotor.set(0);
			rightMotor.set(0);
		} else {
			leftMotor.set(motorSpeed);
			rightMotor.set(motorSpeed);
		}
	}
	
	/**
	 * Constrains a number between min and max. Like the LabVIEW
	 * "Coerce/In-range" VI.
	 * 
	 * @param a
	 * @param min
	 * @param max
	 * @return
	 */
	public static double coerce(double a, double min, double max) {
		return Math.min(max, Math.max(a, min));
	}
	
	public void adjustElevator(double speed){
		//System.out.println(speed);
		setPower(speed);
	}
	

	@Override
	public double pidGet() {
		return getHeight();
	}

	@Override
	public void pidWrite(double output) {
		//System.out.println(output);
		setPower(output);
		
	}

	@Override
	protected void initDefaultCommand() {
		setDefaultCommand(new StopElevator());
		// TODO Auto-generated method stub
		
	}

	@Override
	public boolean isOld() {
		// TODO Auto-generated method stub
		return false;
	}
}