Stefan Krickl / Controller

bla

Pid_control.cpp@0:7ab090cd6520, 2017-07-13 (annotated)

Committer:
skrickl
Date:
Thu Jul 13 13:42:50 2017 +0000
Revision:
0:7ab090cd6520
bla

Who changed what in which revision?

UserRevisionLine numberNew contents of line
skrickl 0:7ab090cd6520 1 #include "Pid_control.h"
skrickl 0:7ab090cd6520 2
skrickl 0:7ab090cd6520 3 Pid_control::Pid_control()
skrickl 0:7ab090cd6520 4 {
skrickl 0:7ab090cd6520 5
skrickl 0:7ab090cd6520 6 derivate =0;
skrickl 0:7ab090cd6520 7 sum=0.0;
skrickl 0:7ab090cd6520 8 delta_t= 0.1;
skrickl 0:7ab090cd6520 9 actual_value_old=0;
skrickl 0:7ab090cd6520 10 }
skrickl 0:7ab090cd6520 11
skrickl 0:7ab090cd6520 12 Pid_control::~Pid_control()
skrickl 0:7ab090cd6520 13 {
skrickl 0:7ab090cd6520 14
skrickl 0:7ab090cd6520 15
skrickl 0:7ab090cd6520 16 }
skrickl 0:7ab090cd6520 17
skrickl 0:7ab090cd6520 18
skrickl 0:7ab090cd6520 19 float Pid_control::control( int actual_value, int target_value, float p, float i,float d, float min_control_out, float max_control_out )
skrickl 0:7ab090cd6520 20 {
skrickl 0:7ab090cd6520 21
skrickl 0:7ab090cd6520 22
skrickl 0:7ab090cd6520 23
skrickl 0:7ab090cd6520 24 error = target_value - actual_value;
skrickl 0:7ab090cd6520 25
skrickl 0:7ab090cd6520 26 // I Anteil
skrickl 0:7ab090cd6520 27
skrickl 0:7ab090cd6520 28 sum = ((float) error) + sum;
skrickl 0:7ab090cd6520 29
skrickl 0:7ab090cd6520 30 // D Anteil
skrickl 0:7ab090cd6520 31
skrickl 0:7ab090cd6520 32 derivate = ((float)(actual_value - actual_value_old))/(delta_t);
skrickl 0:7ab090cd6520 33
skrickl 0:7ab090cd6520 34 // PID Control
skrickl 0:7ab090cd6520 35
skrickl 0:7ab090cd6520 36 control_out= p * ((float) error) + i*sum - d*derivate;
skrickl 0:7ab090cd6520 37
skrickl 0:7ab090cd6520 38
skrickl 0:7ab090cd6520 39
skrickl 0:7ab090cd6520 40 if(control_out > max_control_out )
skrickl 0:7ab090cd6520 41 {
skrickl 0:7ab090cd6520 42 if(i > 0)
skrickl 0:7ab090cd6520 43 {
skrickl 0:7ab090cd6520 44 sum = sum - (control_out - max_control_out)/i;
skrickl 0:7ab090cd6520 45 }
skrickl 0:7ab090cd6520 46 control_out = max_control_out;
skrickl 0:7ab090cd6520 47
skrickl 0:7ab090cd6520 48 }
skrickl 0:7ab090cd6520 49 else if (control_out < min_control_out )
skrickl 0:7ab090cd6520 50 {
skrickl 0:7ab090cd6520 51 control_out = min_control_out;
skrickl 0:7ab090cd6520 52
skrickl 0:7ab090cd6520 53 }
skrickl 0:7ab090cd6520 54
skrickl 0:7ab090cd6520 55
skrickl 0:7ab090cd6520 56
skrickl 0:7ab090cd6520 57 actual_value_old = actual_value;
skrickl 0:7ab090cd6520 58
skrickl 0:7ab090cd6520 59 return control_out;
skrickl 0:7ab090cd6520 60
skrickl 0:7ab090cd6520 61 }