[ODE] Servo to reach a target position

[David Whittaker]

Doh!  And here's the fix to my fix (change the division to multiplication).

> 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;
fix: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
>
>
>
> _______________________________________________
> ODE mailing list
> ODE@q12.org
> http://q12.org/mailman/listinfo/ode