[ODE] Servo to reach a target position

[Royce3]

>I'm be interested!  Not so much for ODE, but because I tried to write one
>for an unrelated project and got nowhere.  Ended up coming up with
>something that does work well, and fits what I thought PID meant, but
>actually looks nothing like what a later learned PID was. :-)

Here's the code, tho I haven't had a chance to test it very extensively.
Be wary the I calculation may be wrong, but it works for me :)

Also remember, 95% of applications only need P, and 99% only need P and D.

// pid.h

#ifndef PID_H
#define PID_H

#include <math.h>

class PID
{
	// Kp = Proportional gain
	// Ki = Integral gain
	// Kd = Derivative gain
	// Isum = accumulated error for I-term
	// E = saved Error from previous calculation
	// MaxE = maximum error signal seen
	// dT1 = saved dT from previous calculation
	// OV = Output Variable
	float Kp, Ki, Kd, Isum, E, MaxE, dT1, OV;
public:
	PID ( float Kp_=0, float Ki_=0, float Kd_=0 )
	{
		Reset();
		SetGain ( Kp_, Ki_, Kd_ );
	}

	void Reset()
	{
		Isum = 0;
		E = 0;
		dT1 = 0;
		MaxE = 0;
		OV = 0;
	}

	void SetGain ( float Kp_, float Ki_=0, float Kd_=0 )
	{
		Kp = Kp_;
		Ki = Ki_;
		Kd = Kd_;
	}

	// SP = destination/target
	// PV = feedback
	// dT = time elapsed since last update
	// return value = command output
	float StepPosition ( float SP, float PV, float dT )
	{
		float E1 = E; // previous error
		E = SP - PV; // new error

		float Op=0, Oi=0, Od=0; // P, I, and D terms of calculation

		// P-Loop
		Op = CalcP ( E );

		// I-Loop
		Oi = CalcI ( E );

		// D-Loop
		Od = CalcD ( E, E1 );

		// Final Summation
		OV = Op + Oi + Od;

		dT1 = dT; // save this dT to be previous dT for next time

		return OV;
	}

	// SP = destination/target
	// PV = feedback
	// dT = time elapsed since last update
	// return value = command output
	float StepSpeed ( float SP, float PV, float dT )
	{
		float E1 = E; // previous error
		E = SP - PV; // new error

		float Op=0, Oi=0, Od=0; // P, I, and D terms of calculation

		// P-Loop
		Op = CalcP ( E );

		// I-Loop
		Oi = CalcI ( E );

		// D-Loop
		Od = CalcD ( E, E1 );

		// Final Summation
		OV += Op + Oi + Od;

		dT1 = dT; // save this dT to be previous dT for next time

		return OV;
	}

private:
	float CalcP ( float E )
	{
		return E * Kp;
	}

	float CalcI ( float E )
	{
		if ( Ki > 0 )
		{
			Isum += E / 10;
			if ( fabs(E) > MaxE )
				MaxE = (float)fabs(E);
			if ( Isum > MaxE )
				Isum = MaxE;
			else if ( Isum < -MaxE )
				Isum = -MaxE;
			return Ki * Isum;
		}
		else
			return 0;
	}

	float CalcD ( float E, float E1 )
	{
		if ( Kd > 0 )
		{
			float dE = E - E1; // change in error
			return Kd * E;
		}
		else
			return 0;
	}
};

#endif//PID_H

// pidtest.cpp

#include <stdio.h>
#include "../pid.h"

void main()
{
	PID pid ( 0.1f );
	float position = 0;
	for ( int i = 0; i < 50; i++ )
	{
		position += pid.StepPosition ( 10, position, 0.1f );
		printf ( "%.02f ", position );
	}
	printf ( "\n\nfinal position: %.02f\n", position );
}

// end of pidtest.cpp