@@ -0,0 +1,2512 @@
+/*%0A * To change this template, choose Tools %7C Templates%0A * and open the template in the editor.%0A */%0Apackage com.team254.lib.control;%0A%0Aimport edu.wpi.first.wpilibj.PIDController;%0Aimport edu.wpi.first.wpilibj.PIDOutput;%0Aimport edu.wpi.first.wpilibj.PIDSource;%0Aimport edu.wpi.first.wpilibj.Timer;%0Aimport java.util.TimerTask;%0A%0A/**%0A *%0A * @author tombot%0A */%0Apublic class ProfiledPIDController %7B%0A    int i = 0;%0A    %0A    public PIDController controller;%0A    java.util.Timer m_controlLoop;%0A    double m_period;%0A    double accel;%0A    double vel;%0A    double t_to_max_v;%0A    double t_from_max_v;%0A    double t_total;%0A    double m_setpoint;%0A    Timer timer = new Timer();%0A    PIDSource m_source;%0A%0A    private class PIDTask extends TimerTask %7B%0A%0A        private ProfiledPIDController m_controller;%0A%0A        public PIDTask(ProfiledPIDController controller) %7B%0A            if (controller == null) %7B%0A                throw new NullPointerException(%22Given ProfiledPIDController was null%22);%0A            %7D%0A            m_controller = controller;%0A        %7D%0A%0A        public void run() %7B%0A            m_controller.calculate();%0A        %7D%0A    %7D%0A%0A    public ProfiledPIDController(double Kp, double Ki, double Kd, double Kf,%0A            PIDSource source, PIDOutput output,%0A            double period) %7B%0A        m_period = period;%0A        controller = new PIDController(Kp, Ki,Kd, Kf, source, output,period);%0A        m_controlLoop.schedule(new PIDTask(this), 0L, (long) (m_period * 1000));%0A        m_source = source;%0A    %7D%0A    %0A    private void calculate() %7B%0A        double t = timer.get();%0A        double setpoint = controller.getSetpoint();%0A        if (t %3C t_to_max_v) %7B // accelerate up%0A            setpoint += (accel * t) * m_period;%0A        %7D%0A        else if (t %3C t_from_max_v) %7B // max v%0A            setpoint += vel * m_period;%0A        %7D%0A        else if (t %3C t_total) %7B // accelerate down%0A            double m_t = t - t_from_max_v;%0A            double v = vel + (-accel * m_t);%0A            setpoint += v * m_period;%0A        %7D%0A        controller.setSetpoint(setpoint);%0A    %7D%0A    %0A    public synchronized void setSetpoint(double setpoint) %7B%0A      m_setpoint = setpoint;%0A      t_to_max_v = vel / accel;%0A      double delta_s_max_v = setpoint - (t_to_max_v * vel);%0A      double t_at_max_v = delta_s_max_v / vel;%0A      t_from_max_v = t_to_max_v + t_at_max_v;%0A      t_total = t_from_max_v + t_to_max_v;    %0A      timer.reset();%0A      %0A      // Set setpoint to current value of PIDSource%0A      controller.setSetpoint(m_source.pidGet());%0A    %7D%0A    %0A%7D%0A

Add ProfiledPIDController