Gal Yaroslavsky / MicroMouse-v1

asdf

Dependencies:   L3GD20 LSM303DLHC mbed

Headers/PID.h@8:ce5b1bf38077, 2014-04-03 (annotated)

Committer:
goy5022
Date:
Thu Apr 03 23:58:04 2014 +0000
Revision:
8:ce5b1bf38077
Parent:
7:95ebadc83fc7 
asdf

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 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