Thanking you in advance......


This is both a request for help but the answer should be rather useful to the community as a important bit of control/stability library code

Background: We are running a relatively complex hydraulic hybrid drive on a a prototype truck with a older Cummins engine that uses a analog throttle. When we set up the throttle as per elsewhere on the site, it runs well, but when we break the link to the engine ECU, we pick up a natural resonance between the reading of the "pedal" and the writing of the cummins "throttle"

While we will be playing with timing and gains etc, we know from the original control algorithm developed in Matlab/Simulink that we always needed a PID ( in this case PID is "Proportional-Integral-Derivative (PID)"). It is a bit of a unfortunate name in the j1939 message context and that is why it is clarified here

For anyone who has a stability problem, this is usually the "goto method" for many designers

The details for the Proportional-Integral-Derivative (PID) Controller code is as below, a direct copy from https://nicisdigital.wordpress.com/2011 ... ontroller/


""
typedef struct {
double windup_guard;
double proportional_gain;
double integral_gain;
double derivative_gain;
double prev_error;
double int_error;
double control;
} PID;

void pid_zeroize(PID* pid) {
// set prev and integrated error to zero
pid->prev_error = 0;
pid->int_error = 0;
}

void pid_update(PID* pid, double curr_error, double dt) {
double diff;
double p_term;
double i_term;
double d_term;

// integration with windup guarding
pid->int_error += (curr_error * dt);
if (pid->int_error < -(pid->windup_guard))
pid->int_error = -(pid->windup_guard);
else if (pid->int_error > pid->windup_guard)
pid->int_error = pid->windup_guard;

// differentiation
diff = ((curr_error - pid->prev_error) / dt);

// scaling
p_term = (pid->proportional_gain * curr_error);
i_term = (pid->integral_gain * pid->int_error);
d_term = (pid->derivative_gain * diff);

// summation of terms
pid->control = p_term + i_term + d_term;

// save current error as previous error for next iteration
pid->prev_error = curr_error;
}

"

end of copy


Please may I ask the Murphy team for a translation to angelscript but also say this is one of the most useful pieces of code to control many physical systems ( by playing with the relative values) and should be a useful asset to the library when translated

Regards
NormanButchGrant

Here is an AngelScript PID controller, translated from arduino code, from http://brettbeauregard.com/blog/category/pid/coding/ which is excellent introduction. Simple translate by Zodio.

There are six total scripts, six of which must be run every tick, and in order (it worked only in this order, your guess is as good as mine as to why). Any suggestions to improve are welcome.

All settings required to tune and run are in PID Compute.

ORDER:
1- PID Compute
2- PID SetTunings
3- PID SetSampleTime
4- PID SetOutputLimits
5- PID SetMode
6- PID SetControllerDirection

If you are looking for this code, then you are aware of the power of this code to stabilize anything from external pump, slewing gear, turret, drilling machine etc without needed to know much about the process. Enjoy.

START ONE

double Setpoint; // Control signal (“Acc_Desired_RPM” in example code)
double Input; // Feedback signal (“J1939_Engine_Engine_Speed” in example code)
double error; // Calculation value to be processed
double Output; // Output signal that has been processed (“Demand” in example code)
double ITerm;
double lastInput;
double kp = 0.25; // Set Proportional gain here
double ki = 1; // Set Integral gain here
double kd = 1; // Set Derivative gain here
int SampleTime = 50; // 50 ms !Set to your tick timer!
double outMin = -30; // Set minimum limit for output signal here
double outMax = 30; // Set maximum limit for output signal here
bool inAuto = false;

/* Changed from #DEFINE, haven't tested AUTOMATIC mode, included for completeness */
int MANUAL = 1;
int AUTOMATIC = 0;


/* Changed from #DEFINE, haven't tested reverse, included for completeness */
int DIRECT = 0;
int REVERSE = 1;

int controllerDirection = DIRECT;

void $PID Compute Event$ ()
{
int timeChange = SampleTime; // Just to get the loop to trigger, no equivalent of millis() available
if(timeChange>=SampleTime)
{
smRead(VariableIDs.Acc_Desired_RPM,Setpoint); // Read your Setpoint value
smRead(VariableIDs.J1939_Engine_Engine_Speed,Input); // Read your feedback value

/* Compute all the working variables */
error = (Setpoint - Input);
ITerm += (ki * error);
if(ITerm > outMax)
{
ITerm = outMax;
}
else if(ITerm < outMin)
{
ITerm = outMin;
}

double dInput = (Input - lastInput);

/* Compute PID Output */
Output = kp * error + ITerm - kd * dInput;
if(Output > outMax)
{
Output = outMax;
}
else if (Output < outMin)
{
Output = outMin;
}

/* Remember some variables for next time */
lastInput = Input;
}
smWrite(VariableIDs.Demand,Output); // Write the processed value to your variable
}

END ONE

START TWO

void $PID SetTunings Event$ (double Kp, double Ki, double Kd)
{
Kp = kp;
Ki = ki;
Kd = kd;
if (Kp<0 or Ki<0 or Kd<0) return;

double SampleTimeInSec = SampleTime/1000;
kp = Kp;
ki = Ki * SampleTimeInSec;
kd = Kd / SampleTimeInSec;

if(controllerDirection == REVERSE)
{
kp = (0 - kp);
ki = (0 - ki);
kd = (0 - kd);
}
}

END TWO

START THREE

void $PID SetSampleTime Event$ (int NewSampleTime)
{
if (NewSampleTime > 0)
{
double ratio = (NewSampleTime)/(SampleTime);

ki *= ratio;
kd /= ratio;
SampleTime = uint64(NewSampleTime);
}
}

END THREE

START FOUR

void $PID SetOutputLimits Event$ (double Min, double Max)
{
Min = outMin;
Max = outMax;
if(Min > Max) return;
outMin = Min;
outMax = Max;

if(Output > outMax)
{
Output = outMax;
}
else if(Output < outMin)
{
Output = outMin;
}

if(ITerm > outMax)
{
ITerm = outMax;
}
else if(ITerm < outMin)
{
ITerm = outMin;
}
}

END FOUR

START FIVE

void $PID SetMode Event$ (int Mode)
{
bool newAuto = (Mode == AUTOMATIC);
if(newAuto == !inAuto)
{ /* we just went from manual to auto */
lastInput = Input;
ITerm = Output;
if(ITerm > outMax) ITerm = outMax;
else if (ITerm < outMin) ITerm = outMin;
}
inAuto = newAuto;
}

END FIVE

START SIX

void $PID SetControllerDirection Event$ (int Direction)
{
controllerDirection = Direction;
}

END SIX