Gal Yaroslavsky / MicroMouse-v1

asdf

Dependencies:   L3GD20 LSM303DLHC mbed

Headers/PID.h@0:c2ec30f28676, 2014-03-29 (annotated)

Committer:
goy5022
Date:
Sat Mar 29 03:06:46 2014 +0000
Revision:
0:c2ec30f28676
Child:
1:cfe6a6ad8dca
dcsddf

Who changed what in which revision?

UserRevisionLine numberNew contents of line
goy5022 0:c2ec30f28676 1 #ifndef PID_H
goy5022 0:c2ec30f28676 2 #define PID_H
goy5022 0:c2ec30f28676 3
goy5022 0:c2ec30f28676 4 #define P_TERM 1
goy5022 0:c2ec30f28676 5 #define I_TERM 0
goy5022 0:c2ec30f28676 6 #define D_TERM 20
goy5022 0:c2ec30f28676 7
goy5022 0:c2ec30f28676 8 float proportional = 0;
goy5022 0:c2ec30f28676 9 float position = 0;
goy5022 0:c2ec30f28676 10 float derivative = 0;
goy5022 0:c2ec30f28676 11 float integral = 0;
goy5022 0:c2ec30f28676 12 float prev = 0;
goy5022 0:c2ec30f28676 13
goy5022 0:c2ec30f28676 14
goy5022 0:c2ec30f28676 15 float PID(float readR, float readL)
goy5022 0:c2ec30f28676 16 {
goy5022 0:c2ec30f28676 17 float error = 0.0f;
goy5022 0:c2ec30f28676 18
goy5022 0:c2ec30f28676 19 position = readR - readL;
goy5022 0:c2ec30f28676 20
goy5022 0:c2ec30f28676 21 proportional = position;
goy5022 0:c2ec30f28676 22 derivative = position - prev;
goy5022 0:c2ec30f28676 23 integral += proportional;
goy5022 0:c2ec30f28676 24 prev = position;
goy5022 0:c2ec30f28676 25
goy5022 0:c2ec30f28676 26 error = (proportional*(P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
goy5022 0:c2ec30f28676 27
goy5022 0:c2ec30f28676 28
goy5022 0:c2ec30f28676 29
goy5022 0:c2ec30f28676 30 return error; // - means left higher, + means right higher
goy5022 0:c2ec30f28676 31 }
goy5022 0:c2ec30f28676 32
goy5022 0:c2ec30f28676 33 /*
goy5022 0:c2ec30f28676 34 position = Left.read() - Right.read(); //PID control
goy5022 0:c2ec30f28676 35 proportional = position;
goy5022 0:c2ec30f28676 36 derivative = position - prev_pos;
goy5022 0:c2ec30f28676 37 integral += proportional;
goy5022 0:c2ec30f28676 38 prev_pos = position;
goy5022 0:c2ec30f28676 39 power = (proportional*(P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
goy5022 0:c2ec30f28676 40 */
goy5022 0:c2ec30f28676 41
goy5022 0:c2ec30f28676 42 #endif