asdf
Dependencies: L3GD20 LSM303DLHC mbed
Headers/PID.h@7:95ebadc83fc7, 2014-04-03 (annotated)
- Committer:
- goy5022
- Date:
- Thu Apr 03 23:57:31 2014 +0000
- Revision:
- 7:95ebadc83fc7
- Parent:
- 6:6e96e93689df
gal's about to fuck with it
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 | 6:6e96e93689df | 4 | #define P_TERM 5 |
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 | 7:95ebadc83fc7 | 10 | #include "Motors.h" |
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 | 6:6e96e93689df | 18 | float PIDv = 0; |
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 | 7:95ebadc83fc7 | 35 | void pid() |
goy5022 | 7:95ebadc83fc7 | 36 | { |
goy5022 | 7:95ebadc83fc7 | 37 | |
goy5022 | 7:95ebadc83fc7 | 38 | if ( wallRight() && wallLeft()) |
goy5022 | 7:95ebadc83fc7 | 39 | PIDv = PID(linearize(SenseR.read()), linearize(SenseL.read())); |
goy5022 | 7:95ebadc83fc7 | 40 | if ( wallRight() && !wallLeft()) |
goy5022 | 7:95ebadc83fc7 | 41 | PIDv = PID(linearize(SenseR.read()),cal_R); |
goy5022 | 7:95ebadc83fc7 | 42 | if ( !wallRight() && wallLeft()) |
goy5022 | 7:95ebadc83fc7 | 43 | PIDv = PID(cal_L, linearize(SenseL.read())); |
goy5022 | 7:95ebadc83fc7 | 44 | |
goy5022 | 7:95ebadc83fc7 | 45 | if(PIDv < -0.1) |
goy5022 | 7:95ebadc83fc7 | 46 | { |
goy5022 | 7:95ebadc83fc7 | 47 | setRightSpeed(3); |
goy5022 | 7:95ebadc83fc7 | 48 | setLeftSpeed(4); |
goy5022 | 7:95ebadc83fc7 | 49 | wait_ms(20); |
goy5022 | 7:95ebadc83fc7 | 50 | } |
goy5022 | 7:95ebadc83fc7 | 51 | else if(PIDv > .1) |
goy5022 | 7:95ebadc83fc7 | 52 | { |
goy5022 | 7:95ebadc83fc7 | 53 | setRightSpeed(4); //** Just flipped these values |
goy5022 | 7:95ebadc83fc7 | 54 | setLeftSpeed(3); |
goy5022 | 7:95ebadc83fc7 | 55 | wait_ms(20); |
goy5022 | 7:95ebadc83fc7 | 56 | } |
goy5022 | 7:95ebadc83fc7 | 57 | else |
goy5022 | 7:95ebadc83fc7 | 58 | { |
goy5022 | 7:95ebadc83fc7 | 59 | setRightSpeed(3); |
goy5022 | 7:95ebadc83fc7 | 60 | setLeftSpeed(3); |
goy5022 | 7:95ebadc83fc7 | 61 | } |
goy5022 | 7:95ebadc83fc7 | 62 | |
goy5022 | 7:95ebadc83fc7 | 63 | } |
goy5022 | 0:c2ec30f28676 | 64 | #endif |