File content as of revision 8:ce5b1bf38077:

#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