File content as of revision 5:0975b797bf54:

//#include "pid.h"
//
//void PIDController::onLoop()
//{
//    float error = (m_error)();
//    (m_out)(m_Kp * error + m_Ki * iController(error) + m_Kd * dController(error));
//    wait(m_dt);
//}
//
//float PIDController::iController(float error)
//{
//    m_integral += error;
//    return m_integral;
//}
//
//float PIDController::dController(float error)
//{
//    float derivative = error - m_pError;
//    m_pError = error;
//    return derivative; 
//}
//
//void PIDController::reset()
//{
//    m_integral = 0;
//}


#include "pid.h"
Timer timer;

void ctrl::setKp(float num){
    
  kp=num;
}
    
    
void ctrl::setKi(float num){
    
  ki=num;
}
void ctrl::setKd(float num){
    
  kd=num;
}
    
void ctrl::setErr(float num){
    
  error=num;
}
    
void ctrl::setprevErr(float num){
    
  prevErr=num;
}
    
void ctrl::setCorrection(float num){
    
  correction=num;
}    

float ctrl::pctrl(){
  correction=kp*error;
  return correction;
    
} 
    
float ctrl::dctrl(){
  float dErr=error-prevErr;
  float dt=timer.read_us();
  timer.reset();
  setprevErr(error);
  correction=kd*dErr/dt;
    
    
  return correction;
    
}
    
    
float ctrl::ictrl(){
  floategrater+=error;
    
  correction=ki*floategrater;
  floategrater/=decay;
  return correction;
    
}
    
float ctrl::total(){
  return pctrl()+dctrl()+ictrl();
    
    
    
    
}
    
void ctrl::adjust(float xSpeed,float ySpeed,float L, float R){
    
    
   TErr=xSpeed-(L+R);
   RErr=rotationError();  
    
    }

float ctrl::getT(){
    
    return TErr;
    }
    
    
float ctrl::getR(){
    return RErr;
    
    }