package org.usfirst.frc.team2791.util;

import org.usfirst.frc.team2791.overridenClasses.BasicPID;

import java.util.Arrays;

/**
 * Created by Akhil on 4/13/2016.
 * This class uses the twiddle algorithm to calculate PID gains
 * This is based on theory and has yet to be tested
 */
public class AutomaticPIDTuner implements Runnable {
    Thread tunerThread;
    AutomaticPIDTuner tuner;
    BasicPID controller;
    boolean running = true;
    //error tolerance
    private double tolerence;
    private double bestError;
    //calculated pid values
    private double[] pid = {0, 0, 0};

    private double[] pidIncrementer = {.1, .1, .1};


    private AutomaticPIDTuner() {
        tolerence = 0.0001;
    }

    public AutomaticPIDTuner getInstance() {
        if (tuner == null)
            tuner = new AutomaticPIDTuner();
        if (tunerThread == null) {
            tunerThread = new Thread();
        }
        return tuner;
    }

    public void setPIDController(BasicPID PIDController) {
        controller = PIDController;
    }

    public void setTolerence(double newTolerence) {
        tolerence = newTolerence;
    }

    public void destroy() {
        running = false;
        tuner = null;
        tunerThread = null;
    }

    public void calculateAndUpdateGains(double setpoint) {
        //keep running until the imcrementers are near 0
        double currentError = 100;
        while (pidIncrementer[0] + pidIncrementer[1] + pidIncrementer[2] > tolerence) {
            for (int x = 0; x < 3; x++) {
                pid[x] += pidIncrementer[x];
                controller.changeGains(pid[0], pid[1], pid[2]);
                controller.setSetPoint(setpoint);
                sleep(5000);
                currentError = controller.getError();
                if (currentError < bestError) {
                    bestError = currentError;
                    pidIncrementer[x] *= 1.1;
                } else {
                    pid[x] -= 2.0 * pidIncrementer[x];
                    controller.changeGains(pid[0], pid[1], pid[2]);
                    controller.setSetPoint(0);
                    sleep(5000);
                    currentError = controller.getError();
                    if (currentError < bestError) {
                        bestError = currentError;
                        pidIncrementer[x] *= 1.1;
                    } else {
                        pid[x] += pidIncrementer[x];
                        pidIncrementer[x] *= 0.9;
                    }
                }

            }
            System.out.println("Automatic Tuning Results: PID values: " + Arrays.toString(pid) + " Current error: "
                    + currentError + " Best error " + bestError);
        }
    }

    private void sleep(long millis) {
        try {
            Thread.sleep(millis);
        } catch (InterruptedException e) {
            e.printStackTrace();
        }
    }

    public void start() {
        tunerThread.start();
    }

    public void run() {
        double multiplier = -1.1;
        double setpoint = 10;
        while (running) {
            calculateAndUpdateGains(setpoint*=multiplier);
        }
    }
}


@@ -1,3171 +1,18 @@
-package org.usfirst.frc.team2791.util;%0A%0Aimport org.usfirst.frc.team2791.overridenClasses.BasicPID;%0A%0Aimport java.util.Arrays;%0A%0A/**%0A * Created by Akhil on 4/13/2016.%0A * This class uses the twiddle algorithm to calculate PID gains%0A * This is based on theory and has yet to be tested%0A */%0Apublic class AutomaticPIDTuner implements Runnable %7B%0A    Thread tunerThread;%0A    AutomaticPIDTuner tuner;%0A    BasicPID controller;%0A    boolean running = true;%0A    //error tolerance%0A    private double tolerence;%0A    private double bestError;%0A    //calculated pid values%0A    private double%5B%5D pid = %7B0, 0, 0%7D;%0A%0A    private double%5B%5D pidIncrementer = %7B.1, .1, .1%7D;%0A%0A%0A    private AutomaticPIDTuner() %7B%0A        tolerence = 0.0001;%0A    %7D%0A%0A    public AutomaticPIDTuner getInstance() %7B%0A        if (tuner == null)%0A            tuner = new AutomaticPIDTuner();%0A        if (tunerThread == null) %7B%0A            tunerThread = new Thread();%0A        %7D%0A        return tuner;%0A    %7D%0A%0A    public void setPIDController(BasicPID PIDController) %7B%0A        controller = PIDController;%0A    %7D%0A%0A    public void setTolerence(double newTolerence) %7B%0A        tolerence = newTolerence;%0A    %7D%0A%0A    public void destroy() %7B%0A        running = false;%0A        tuner = null;%0A        tunerThread = null;%0A    %7D%0A%0A    public void calculateAndUpdateGains(double setpoint) %7B%0A        //keep running until the imcrementers are near 0%0A        double currentError = 100;%0A        while (pidIncrementer%5B0%5D + pidIncrementer%5B1%5D + pidIncrementer%5B2%5D %3E tolerence) %7B%0A            for (int x = 0; x %3C 3; x++) %7B%0A                pid%5Bx%5D += pidIncrementer%5Bx%5D;%0A                controller.changeGains(pid%5B0%5D, pid%5B1%5D, pid%5B2%5D);%0A                controller.setSetPoint(setpoint);%0A                sleep(5000);%0A                currentError = controller.getError();%0A                if (currentError %3C bestError) %7B%0A                    bestError = currentError;%0A                    pidIncrementer%5Bx%5D *= 1.1;%0A                %7D else %7B%0A                    pid%5Bx%5D -= 2.0 * pidIncrementer%5Bx%5D;%0A                    controller.changeGains(pid%5B0%5D, pid%5B1%5D, pid%5B2%5D);%0A                    controller.setSetPoint(0);%0A                    sleep(5000);%0A                    currentError = controller.getError();%0A                    if (currentError %3C bestError) %7B%0A                        bestError = currentError;%0A                        pidIncrementer%5Bx%5D *= 1.1;%0A                    %7D else %7B%0A                        pid%5Bx%5D += pidIncrementer%5Bx%5D;%0A                        pidIncrementer%5Bx%5D *= 0.9;%0A                    %7D%0A                %7D%0A%0A            %7D%0A            System.out.println(%22Automatic Tuning Results: PID values: %22 + Arrays.toString(pid) + %22 Current error: %22%0A                    + currentError + %22 Best error %22 + bestError);%0A        %7D%0A    %7D%0A%0A    private void sleep(long millis) %7B%0A        try %7B%0A            Thread.sleep(millis);%0A        %7D catch (InterruptedException e) %7B%0A            e.printStackTrace();%0A        %7D%0A    %7D%0A%0A    public void start() %7B%0A        tunerThread.start();%0A    %7D%0A%0A    public void run() %7B%0A        double multiplier = -1.1;%0A        double setpoint = 10;%0A        while (running) %7B%0A            calculateAndUpdateGains(setpoint*=multiplier);%0A        %7D%0A    %7D%0A%7D%0A
+//still underway 
 %0A

Update AutomaticPIDTuner.java