*

-- Author.

import java.util.*;
public class pidsim 
{		
	double timeunit = 0.02;      //iteration time (guesstimated)
	double maxrotaterate = 1.00; //maximum rotation rate allowed 
	
	//play with the following 3 constants
	double kp = 1.5;             //proportional constant
	double ki = 1.0;             //integral constant
	double kd = 1.0;             //derivative constant
	
	double setpoint = 0;         //target angle
	double maxturnrate = 0.1;    //guess at motor maximum turning rate
	double accuracy = 0.50;      //tolerance of target closeness 
	double currheading = 0;      //current reported gyro heading
	double error = 0.0;          //difference (heading - setpoint)
	double preverror = 0;        //prior error calculated 
	double rr = 0.0;             //rotation rate
	double i = 0.0;              //integral variable
	double d = 0.0;              //derivative variable
	
	static int src = 0;
	int rc = 0;                  //return code
	int approachside = 0;        //1 = postive slope, -1 = negative slope
	int icount = 0;
	

public static void main(String[] args) 
{
	pidsim t = new pidsim();
	src = t.process();
	System.out.println("E-O-P");
	return;
}

	
public int process()
{
	String keyline;
	Scanner sc = new Scanner(System.in);
	for(;;)
	{
    	System.out.println("Please enter target angle");
    	keyline = sc.nextLine();
    	keyline = keyline.trim();
    	if(keyline.length() == 0)
        {
            System.out.println("Nothing entered, terminating");
            break;
        }
        try
        {
            setpoint = Integer.parseInt((String) keyline);
        }
        catch(NumberFormatException e)
        {
            System.out.println("Non-integer detected");
            continue;
        }
        rc = initvalues();
        rc = pidprocess();
	}
	sc.close();
	return 0;
} // end method


public int pidprocess()
{
	if ( (setpoint - currheading) > 0 )    //Recognize overshoot of target from any oscillation
	{
		approachside = 1;
	}
	else
	{
		approachside = -1;
	}
	while (Math.abs(setpoint - currheading) > accuracy)
	{
		currheading = currheading + (Math.abs(setpoint) * maxturnrate * rr);  //a guess about amount of movement
		rr = calcPID();
        //pause(1000);		
	}
	return 0;
} // end method


public double calcPID()
{
	error = setpoint - currheading;      //amount still to go to reach target setpoint
	if (((error > 0) && (preverror < 0)) || ((error < 0) && (preverror > 0)))
	{
		approachside = approachside * -1;
		System.out.println("Setpoint approach direction change");
		i = 0;  //reset integral summation 
	}	
	i = i + (error * timeunit);         //sum of the history of (errors * timeunit)
	double deltaerror = preverror - error;
	if (Math.abs(deltaerror) < timeunit) 	//employ only when change in error is small
	{
		d = deltaerror / timeunit;  //slope of the current error differentiated over timeunit
	}
	else
	{
		d = 0;
	}
	preverror = error;     //save value for next iteration
	rr = (kp * error) + (ki * i) + (kd * d);  //rotation rate calculation
	if (rr > 0)                               //constrain on the interval (-1,1) or less
	{
		rr = Math.min((rr / Math.abs(setpoint)), maxrotaterate);
	}
	else
	{
		rr = Math.max((rr / Math.abs(setpoint) ), -1 * maxrotaterate);
	}	
	System.out.printf("Interval: %3d  Heading: %+7.2f  Error: %+7.2f  Integral: %+6.2f  Derivative: %+5.2f  RotateRate: %+5.2f ",icount,currheading,error,i,d,rr);
	System.out.println("");
	icount++;
	return rr;
} // end method


public int initvalues()      //Initialize variables
{
	icount = 0;
	currheading = 0;
	error = 0.0;
	preverror = 0;
	rr = 0.0;
	i = 0;
	d = 0;
	approachside = 0;
	return 0;
} // end method


public static int pause(int d)     // puts thread to sleep for d milliseconds
{ 
	try
	{
		Thread.sleep(d);
	}
	catch (InterruptedException e1)
	{
		return 1;
	}
    return 0;
}


} // end class

} // end class

</pre>