package com.team254.frc2015.subsystems.controllers;

import com.team254.lib.util.Controller;
import com.team254.lib.util.SynchronousPID;

public class ElevatorCarriageCurrentController extends Controller {
	SynchronousPID m_pid;
	boolean m_go_up;

	public ElevatorCarriageCurrentController(double kp, double ki, double kd,
			double max_output) {
		m_pid = new SynchronousPID(kp, ki, kd);
		m_pid.setOutputRange(-Math.abs(max_output), Math.abs(max_output));
	}

	public void setGoal(boolean push_upwards, double goal) {
		m_pid.setSetpoint(goal);
		if (m_go_up != push_upwards) {
			// Direction has changed.
			reset();
			m_go_up = push_upwards;
		}
	}

	public double update(double measured) {
		double output = m_pid.calculate(measured);
		if (!m_go_up) {
			output *= -1;
		}
		return output;
	}

	@Override
	public void reset() {
		m_pid.reset();
	}

	@Override
	public boolean isOnTarget() {
		// This is a "best effort" controller
		return true;
	}

}

@@ -222,16 +222,56 @@
  m_pid;%0A
+%09double m_max_output;%0A%09double m_output;%0A
 %09boolean
@@ -430,59 +430,21 @@
 %09%09m_
-pid.setOutputRange(-Math.abs(max_output), Math.abs(
+max_output = 
 max_
@@ -453,10 +453,8 @@
 tput
-))
 ;%0A%09%7D
@@ -774,16 +774,173 @@
 -1;%0A%09%09%7D%0A
+%09%09m_output += output;%0A%09%09if (m_output %3E m_max_output) %7B%0A%09%09%09m_output = m_max_output;%0A%09%09%7D else if (m_output %3C -m_max_output) %7B%0A%09%09%09m_output = -m_max_output;%0A%09%09%7D%0A
 %09%09return
@@ -940,16 +940,18 @@
 %09return 
+m_
 output;%0A
@@ -984,24 +984,40 @@
 d reset() %7B%0A
+%09%09m_output = 0;%0A
 %09%09m_pid.rese

Fix current control loop