PID.h

00001 #ifndef PID_H
00002 #define PID_H
00003 
00004 #define P_TERM 5
00005 #define I_TERM 0
00006 #define D_TERM 40
00007 
00008 
00009 #include "Sensors.h"
00010 #include "Motors.h"
00011 
00012 
00013 float proportional = 0;
00014 float position = 0;
00015 float derivative = 0;
00016 float integral = 0;
00017 float prev = 0;
00018 float PIDv = 0;
00019 
00020 float PID(float right, float left)
00021 {
00022     //right = log(right);
00023     //left = log(left);
00024     
00025     position = left - right; //accR - accL;
00026                 
00027     proportional = position;
00028     derivative = position - prev;
00029     integral += proportional;
00030     prev = position;
00031     
00032     return (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM)); 
00033 }
00034 
00035 void pid()
00036 {
00037  
00038         if ( wallRight() && wallLeft())
00039         PIDv = PID(linearize(SenseR.read()), linearize(SenseL.read()));
00040         if ( wallRight() && !wallLeft())
00041         PIDv = PID(linearize(SenseR.read()),cal_R);
00042         if ( !wallRight() && wallLeft())
00043         PIDv = PID(cal_L, linearize(SenseL.read()));
00044         
00045         if(PIDv < -0.1)
00046         {
00047             setRightSpeed(3);
00048             setLeftSpeed(4);
00049             wait_ms(20); 
00050         }
00051         else if(PIDv > .1)
00052         {
00053             setRightSpeed(4); //** Just flipped these values
00054             setLeftSpeed(3);
00055             wait_ms(20);        
00056         }
00057         else
00058         {
00059             setRightSpeed(3);
00060             setLeftSpeed(3);
00061         } 
00062     
00063 }
00064 #endif