[ODE] Servo to reach a target position

[David Whittaker]

Noticed a bug.... see below (the fix:'s).  Unless I'm really confused, you
need to divide the Error by the timestep instead of a constant 10 in the
CalcI function.

Conceptually, it seems to me that the I calculation should only take into
account the integral of the error over the past (N) seconds.  As opposed
to the total error since the dawn of time...  But I could be wrong.

David

>>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 );
fix: 		Oi = CalcI ( E, dT );
>
> 		// 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 );
fix: 		Oi = CalcI ( E, dT );
>
> 		// 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 )
fix: 	float CalcI ( float E, float dT )
> 	{
> 		if ( Ki > 0 )
> 		{
> 			Isum += E / 10;
fix: 			Isum += E / dT;
> 			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
>
> _______________________________________________
> ODE mailing list
> ODE@q12.org
> http://q12.org/mailman/listinfo/ode