pd_disc_f

double angulo = 0;

double pwm = 11;
const int dir1 = 8;
const int dir2 = 9;

double setpoint = 20;
//double Kp = 10.3;
double Kp = 5;
double Kd = 10;//0.0512;

float ultimo_erro = 0;
float erro = 0;
float delta_erro = 0;
float termo_pd = 0;
float termo_pd_escalonado = 0;

const int pot_pin = A0;
int pot = 0;
int pmin = 97, pmax = 982, d2 = 180, d1 = 0;
void setup() {
  Serial.begin(9600);

  pinMode(pwm, OUTPUT);
  pinMode(8, OUTPUT);
  pinMode(9, OUTPUT);
}

void loop(){
  pd_calc();

  if (angulo < setpoint) {
    digitalWrite(8, LOW);
    digitalWrite(9, HIGH);
  }
  else {
    digitalWrite(8, HIGH);
    digitalWrite(9, LOW);
  }

  analogWrite(11, termo_pd_escalonado);
  //Serial.print("ANG");
  //Serial.println(angulo);
  delay(20);
}

void pd_calc(){
  pot = analogRead(pot_pin);
  angulo = map(pot, 0, 1023, 0, 180);
  if (angulo > 165 or angulo < 15){
    Serial.println("FREIO");
    digitalWrite(8, LOW);
    digitalWrite(9, LOW);
    termo_pd_escalonado = 0;
  }
  else {
    erro = setpoint - angulo;
    Serial.print("ERR");
    Serial.println(erro);
    delta_erro = erro - ultimo_erro;
    termo_pd = (Kp * erro) + (Kd * delta_erro);
    termo_pd = constrain(termo_pd, -255, 255);
    termo_pd_escalonado = abs(termo_pd);
    ultimo_erro = erro;
  }
}