PID_Tuned_Code.ino

#include <FreqMeasure.h>
#include <math.h>
int motorA1 = 5 ;
int motorA2 = 6 ;
String STR;
String inByte;
float Speed = 0 ;
float error = 0;
float pTerm, iTerm, dTerm;
float controllerOut = 0;
float setPoint = 40;
float Feedback = 0;
float sumError = 0;
float preError = 0;
float Kp =0.1920*75/12; // 0.32*75/12;
float Ki =38.40*75/12; //0.0005;
float Kd =(2.4/10000)*75/12; //0.001;;

//float Kp =0.32*75/12;
//float Ki =0; //0.0005;
//float Kd =0; //0.001;;


float command = 0;
int count = 0 ;
double sum ;
float N = 32;
void setup() {
  pinMode(5 , OUTPUT) ;
  pinMode(6 , OUTPUT) ;
  Serial.begin(115200) ;
  FreqMeasure.begin();
}
void PID(void);
int a = 0;
int t = 0;
void loop() {
  t += 1;
  if (FreqMeasure.available()){
sum = sum + FreqMeasure.read();
count = count +1 ;
if (count > 30){
  float frequency  = FreqMeasure.countToFrequency(sum/count) ;
  //Serial.println(frequency/500) ;
  Feedback = frequency/500;
  //Serial.println(Feedback) ;
  sum = 0 ;
  count = 0 ;
}
}
  error = (setPoint - Feedback);

  Serial.println(Feedback);
  //Serial.println(millis());
  sumError = sumError + error;
  pTerm = Kp*error;
  iTerm = Ki*sumError;
  dTerm = Kd*(error - preError);
  //dTerm = Kd*N*(1+N*sumError);
  if(iTerm>(setPoint))
  {
    iTerm = 0.9*setPoint;
  }
  preError = error;
  controllerOut = pTerm + iTerm + dTerm;
  if(abs(controllerOut)>75)
  {
      controllerOut = 75;
  }
  Speed = controllerOut/75*255;
    digitalWrite(5 , LOW);
    analogWrite(6 , Speed);
    //delay(0.5);
}