File content as of revision 11:8faa068940f1:

#include "pid.h"



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 time=timer.read_us();
  float dt=time-prevTime;
  prevTime=time;
  
  //pc.printf("time: %f \n",time);
  
  
  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(){
  
  
  pc.printf("p: %f \t d: %f \t i: %f \t \n ", pctrl(),dctrl(),ictrl());
  
  
  return pctrl()+dctrl()+ictrl();
    
    
    
    
}
    
void ctrl::adjust(float xSpeed,float L, float R){
    
    
   TErr=xSpeed-(L+R);
   RErr=rotationError();  
    
    }
    

void ctrl::updateErr(float e){
    
    setprevErr(error);
    error=e;
    
    }


float ctrl::getT(){
    
    return TErr;
    }
    
    
float ctrl::getR(){
    return RErr;
    
    }
    
//#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;
//}