dror balbul / Mbed 2 deprecated two-signalssub

for noam

Dependencies:   mbed Map

PID.c@0:33b00fa05201, 2019-12-20 (annotated)

Committer:
drorbalbul
Date:
Fri Dec 20 15:17:18 2019 +0000
Revision:
0:33b00fa05201
noam

Who changed what in which revision?

UserRevisionLine numberNew contents of line
drorbalbul 0:33b00fa05201 1
drorbalbul 0:33b00fa05201 2 #include "PID.h"
drorbalbul 0:33b00fa05201 3
drorbalbul 0:33b00fa05201 4 pid* pid_create(pid* pid, float* input, float* output, float* setpoint, float kp, float ki, float kd)
drorbalbul 0:33b00fa05201 5 {
drorbalbul 0:33b00fa05201 6 pid->input = input;
drorbalbul 0:33b00fa05201 7 pid->output = output;
drorbalbul 0:33b00fa05201 8 pid->setpoint = setpoint;
drorbalbul 0:33b00fa05201 9 pid->sumError = 0;
drorbalbul 0:33b00fa05201 10 pid->lastInput = 0;
drorbalbul 0:33b00fa05201 11 pid->lastError = 0;
drorbalbul 0:33b00fa05201 12 pid_limits(pid,-254, 254);
drorbalbul 0:33b00fa05201 13
drorbalbul 0:33b00fa05201 14 pid_tune(pid, kp, ki, kd);
drorbalbul 0:33b00fa05201 15
drorbalbul 0:33b00fa05201 16 return pid;
drorbalbul 0:33b00fa05201 17 }
drorbalbul 0:33b00fa05201 18
drorbalbul 0:33b00fa05201 19 void pid_compute(pid* pid)
drorbalbul 0:33b00fa05201 20 {
drorbalbul 0:33b00fa05201 21
drorbalbul 0:33b00fa05201 22
drorbalbul 0:33b00fa05201 23 float in = *(pid->input);
drorbalbul 0:33b00fa05201 24 // Compute error
drorbalbul 0:33b00fa05201 25 float error =(*(pid->setpoint)) - in;
drorbalbul 0:33b00fa05201 26 // Compute integral
drorbalbul 0:33b00fa05201 27 pid->sumError += (pid->Ki * error);
drorbalbul 0:33b00fa05201 28 /*if ((error>0 && pid->lastError<0) || (error<0 && pid->lastError>0) ){
drorbalbul 0:33b00fa05201 29 pid->sumError = 0;
drorbalbul 0:33b00fa05201 30 }*/
drorbalbul 0:33b00fa05201 31 if (pid->sumError > 254)
drorbalbul 0:33b00fa05201 32 pid->sumError = 255;
drorbalbul 0:33b00fa05201 33 else if (pid->sumError < -254)
drorbalbul 0:33b00fa05201 34 pid->sumError = -255;
drorbalbul 0:33b00fa05201 35 // Compute differential on input
drorbalbul 0:33b00fa05201 36 float dinput = in - pid->lastInput;
drorbalbul 0:33b00fa05201 37 // Compute PID output
drorbalbul 0:33b00fa05201 38 float out = pid->Kp * error + pid->sumError - pid->Kd * dinput;
drorbalbul 0:33b00fa05201 39 // Apply limit to output value
drorbalbul 0:33b00fa05201 40 int absOut=abs((int)out);
drorbalbul 0:33b00fa05201 41 if (out > pid->maxOutput){
drorbalbul 0:33b00fa05201 42 out = 255;
drorbalbul 0:33b00fa05201 43 //pid->sumError=0;
drorbalbul 0:33b00fa05201 44 }
drorbalbul 0:33b00fa05201 45 else if (out < pid->minOutput){
drorbalbul 0:33b00fa05201 46 out = -255;
drorbalbul 0:33b00fa05201 47 //pid->sumError=0;
drorbalbul 0:33b00fa05201 48 }
drorbalbul 0:33b00fa05201 49 // Output to pointed variable
drorbalbul 0:33b00fa05201 50 (*pid->output) = out;
drorbalbul 0:33b00fa05201 51 // Keep track of some variables for next execution
drorbalbul 0:33b00fa05201 52 pid->lastInput = in;
drorbalbul 0:33b00fa05201 53 pid->lastError = error;
drorbalbul 0:33b00fa05201 54 }
drorbalbul 0:33b00fa05201 55
drorbalbul 0:33b00fa05201 56 void pid_tune(pid* pid, float kp, float ki, float kd)
drorbalbul 0:33b00fa05201 57 {
drorbalbul 0:33b00fa05201 58 // Check for validity
drorbalbul 0:33b00fa05201 59 pid->Kp = kp;
drorbalbul 0:33b00fa05201 60 pid->Ki = ki ;
drorbalbul 0:33b00fa05201 61 pid->Kd = kd ;
drorbalbul 0:33b00fa05201 62
drorbalbul 0:33b00fa05201 63 }
drorbalbul 0:33b00fa05201 64
drorbalbul 0:33b00fa05201 65
drorbalbul 0:33b00fa05201 66 void pid_limits(pid* pid, float min, float max)
drorbalbul 0:33b00fa05201 67 {
drorbalbul 0:33b00fa05201 68 if (min >= max) return;
drorbalbul 0:33b00fa05201 69 pid->minOutput = min;
drorbalbul 0:33b00fa05201 70 pid->maxOutput = max;
drorbalbul 0:33b00fa05201 71 if (pid->sumError > pid->maxOutput)
drorbalbul 0:33b00fa05201 72 pid->sumError = pid->maxOutput;
drorbalbul 0:33b00fa05201 73 else if (pid->sumError < pid->minOutput)
drorbalbul 0:33b00fa05201 74 pid->sumError = pid->minOutput;
drorbalbul 0:33b00fa05201 75 }
drorbalbul 0:33b00fa05201 76
drorbalbul 0:33b00fa05201 77
drorbalbul 0:33b00fa05201 78
drorbalbul 0:33b00fa05201 79
drorbalbul 0:33b00fa05201 80
drorbalbul 0:33b00fa05201 81
drorbalbul 0:33b00fa05201 82
drorbalbul 0:33b00fa05201 83