Gal Yaroslavsky / MicroMouse-v1

asdf

Dependencies:   L3GD20 LSM303DLHC mbed

Headers/PID.h@2:997f57aee3b7, 2014-04-03 (annotated)

Committer:
goy5022
Date:
Thu Apr 03 13:15:40 2014 +0000
Revision:
2:997f57aee3b7
Parent:
1:cfe6a6ad8dca
Child:
3:1a8a7cc709cc
Gal last touched this inappropriately

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 1:cfe6a6ad8dca 8
goy5022 1:cfe6a6ad8dca 9 #include "Sensors.h"
goy5022 1:cfe6a6ad8dca 10
goy5022 2:997f57aee3b7 11
goy5022 2:997f57aee3b7 12
goy5022 0:c2ec30f28676 13 float proportional = 0;
goy5022 0:c2ec30f28676 14 float position = 0;
goy5022 0:c2ec30f28676 15 float derivative = 0;
goy5022 0:c2ec30f28676 16 float integral = 0;
goy5022 0:c2ec30f28676 17 float prev = 0;
goy5022 0:c2ec30f28676 18
goy5022 0:c2ec30f28676 19
goy5022 2:997f57aee3b7 20 float PID(float right, float left)
goy5022 0:c2ec30f28676 21 {
goy5022 2:997f57aee3b7 22 position = left - right; //accR - accL;
goy5022 0:c2ec30f28676 23
goy5022 2:997f57aee3b7 24 proportional = position;
goy5022 2:997f57aee3b7 25 derivative = position - prev;
goy5022 2:997f57aee3b7 26 integral += proportional;
goy5022 2:997f57aee3b7 27 prev = position;
goy5022 0:c2ec30f28676 28
goy5022 2:997f57aee3b7 29 return (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM));
goy5022 0:c2ec30f28676 30 }
goy5022 0:c2ec30f28676 31
goy5022 0:c2ec30f28676 32
goy5022 0:c2ec30f28676 33 #endif