pd_disc_f2

double angulo = 0;

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

double setpoint = 150;
//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;
const int corr_pin = A1;
int pot = 0;
int corrente = 0;
int mVperAmp = 66;
int ACSoffset = 2500;
double mV = 0;
double Amp = 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);
  corrente = analogRead(corr_pin);
  mV = (corrente / 1024.0) * 5000;
  Amp = ((mV - ACSoffset) / mVperAmp);
  Serial.println(Amp);

  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;
  }
}