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?
User | Revision | Line number | New 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 |