dror balbul / Mbed 2 deprecated pidtaurd

taurd for noam

Dependencies:   mbed Map

PID.c@2:fd1023193cc4, 2019-12-20 (annotated)

Committer:
drorbalbul
Date:
Fri Dec 20 16:29:33 2019 +0000
Revision:
2:fd1023193cc4
pidtaurd

Who changed what in which revision?

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