roboforum.ru

double balanceAt=1.31;//radians
float coeffP=5,coeffI=1,coeffD=2,coeffPID=10;
float lastError=0;
float integral=0;

void loop() {
readAverangeAcc();
 
double actualAngle=comp_filter(getAccAngle(),readAverangeGyroYRate()); 

long pid=updatePID(actualAngle,balanceAt);
lMotorSpeed=-pid;
rMotorSpeed=-pid; 
updateMotors();
lastTime=millis();

delay(10);
}

float updatePID(float current, float target){
   float dt=millis()-lastTime;
 
   float error=target-current;
     
        integral+=error*dt;
        integral=constrain(integral,-10,10);
   float derivative = (error - lastError)*1000/dt;
   Serial.print("D:");
   Serial.println(derivative);
   float output=coeffP*error+coeffI*integral+coeffD*derivative;

   lastError=error;
   
   return constrain(coeffPID*output, -255, 255);
}

/********************************************************************
* Complimentary Filter
********************************************************************/
double filterAngle=1.50;
double dt=0.005;//0.002

double comp_filter(double newAngle, double newRate) {

double filterTerm0;
double filterTerm1;
double filterTerm2;
double timeConstant;

timeConstant=1; // default 1.0

filterTerm0 = (newAngle - filterAngle) * timeConstant * timeConstant;
filterTerm2 += filterTerm0 * dt;
filterTerm1 = filterTerm2 + ((newAngle - filterAngle) * 2 * timeConstant) + newRate;
filterAngle = (filterTerm1 * dt) + filterAngle;

return filterAngle; // This is actually the current angle, but is stored for the next iteration
}