Gal Yaroslavsky / MicroMouse-v1

asdf

Dependencies:   L3GD20 LSM303DLHC mbed

Headers/PID.h@3:1a8a7cc709cc, 2014-04-03 (annotated)

Committer:
goy5022
Date:
Thu Apr 03 15:52:54 2014 +0000
Revision:
3:1a8a7cc709cc
Parent:
2:997f57aee3b7
Child:
6:6e96e93689df
PID is a little spotty but is working. Also handbrake just added

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 3:1a8a7cc709cc 6 #define D_TERM 40
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 3:1a8a7cc709cc 22 //right = log(right);
goy5022 3:1a8a7cc709cc 23 //left = log(left);
goy5022 3:1a8a7cc709cc 24
goy5022 2:997f57aee3b7 25 position = left - right; //accR - accL;
goy5022 0:c2ec30f28676 26
goy5022 2:997f57aee3b7 27 proportional = position;
goy5022 2:997f57aee3b7 28 derivative = position - prev;
goy5022 2:997f57aee3b7 29 integral += proportional;
goy5022 2:997f57aee3b7 30 prev = position;
goy5022 0:c2ec30f28676 31
goy5022 2:997f57aee3b7 32 return (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM));
goy5022 0:c2ec30f28676 33 }
goy5022 0:c2ec30f28676 34
goy5022 0:c2ec30f28676 35
goy5022 0:c2ec30f28676 36 #endif