asdf
Dependencies: L3GD20 LSM303DLHC mbed
Headers/PID.h
- Committer:
- goy5022
- Date:
- 2014-04-03
- Revision:
- 7:95ebadc83fc7
- Parent:
- 6:6e96e93689df
File content as of revision 7:95ebadc83fc7:
#ifndef PID_H #define PID_H #define P_TERM 5 #define I_TERM 0 #define D_TERM 40 #include "Sensors.h" #include "Motors.h" float proportional = 0; float position = 0; float derivative = 0; float integral = 0; float prev = 0; float PIDv = 0; float PID(float right, float left) { //right = log(right); //left = log(left); position = left - right; //accR - accL; proportional = position; derivative = position - prev; integral += proportional; prev = position; return (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM)); } void pid() { if ( wallRight() && wallLeft()) PIDv = PID(linearize(SenseR.read()), linearize(SenseL.read())); if ( wallRight() && !wallLeft()) PIDv = PID(linearize(SenseR.read()),cal_R); if ( !wallRight() && wallLeft()) PIDv = PID(cal_L, linearize(SenseL.read())); if(PIDv < -0.1) { setRightSpeed(3); setLeftSpeed(4); wait_ms(20); } else if(PIDv > .1) { setRightSpeed(4); //** Just flipped these values setLeftSpeed(3); wait_ms(20); } else { setRightSpeed(3); setLeftSpeed(3); } } #endif