Balancín con helicoide para usos prácticos hecho con ARDUINO

   Acá les dejo el programa. Los "m" y "e" se usan dependiendo de la complejidad de la ecuación:

int Salida=3;
int PinAD=0;
int Valor;
double duty;
int Sensor;
int sp;
int Ang;
double ei;
double e[20];
double m[20];
double PWM;

int valor;
int Digito;
int GradosX;

float Kp= 0.01;
float Ki= 0.25;
float Kd= 0;

void setup() {
  pinMode(Salida, OUTPUT);
  Serial.begin(19200);
}

void loop() {
  //if (Serial.available()>0){  //al recibir datos del puerto, introducir referencia
  //  delay(10);
  //  sp = Serial.parseInt();
  //  serialFlush();
  //  if (sp>45){sp=45;}
  //  if (sp<0){sp=0;}
  //  Serial.print("SP= "); Serial.print(sp);
  //}


if (sp != 0) {goto ConPID;}
Serial.print ("Kp= ");Serial.print (Kp);Serial.print (" Ki= ");Serial.print (Ki);Serial.print (" Kd= ");Serial.println (Kd);
Serial.print ("Ingrese grados deseados ");
valor = Serial.read();  //Vaciar el bufer de entrada
Digito=1;
GradosX=0;
  do
  {
while (Serial.available()==0);
valor = Serial.read();
if (valor>47 && valor<58)
   {
      if (Digito==1)
        {
          Digito=2;
          GradosX= valor-48;
          goto ContGr;
        }
      if (Digito==2)
        {
          Digito=3;
          GradosX= GradosX*10 + valor-48;
          goto ContGr;
        }
      if (Digito==3)
        {
          Digito=1;
          GradosX= GradosX*10 + valor-48;
        }
   ContGr:;
      Serial.print ("; Gr="); Serial.print (GradosX);
      Serial.println ();
      Serial.print ("Ingrese grados deseados ");

      //Serial.print (Digito);
      //Serial.println ();
   }
  }while (valor!=13);
  sp=GradosX;
Serial.print ("Gr="); Serial.print (sp);
Serial.println ();
PWM=10;
ConPID:;



 

  Sensor=analogRead(PinAD);
  //Mapear angulo
  Ang=map(Sensor,130,680,0,140);
  Serial.print ("Ang= ");
  Serial.print(Ang);

  //Calcular el error
  ei= sp - Ang;
  Serial.print (" ei= ");
  Serial.print(ei);
 
  //Correr las variables anteriores
    e[3] = e[2];
    e[2] = e[1];
    e[1] = e[0];
    e[0] = ei;
 
    m[6] = m[5];
    m[5] = m[4];
    m[4] = m[3];
    m[3] = m[2];
    m[2] = m[1];
    m[1] = m[0];

  //Colocar la función
  m[0]= m[2] + ((Kp+0.025*Ki+40*Kd)*e[0]) + (0.05*Ki-80*Kd)*e[1] + ((-0.025*Ki- Kp- 40*Kd)*e[2]);
  if (m[0]>175){m[0]=175;}
  if (m[0]<0) {m[0]=0;}

 
  PWM = m[0]*2.25;
  if (PWM>255){PWM=255;}
  if (PWM<0) {PWM=0;}
  analogWrite(Salida,PWM);
  delay(50);
  Serial.print (" PWM= ");
  Serial.println(PWM);
}

void serialFlush(){
    while(Serial.available()>0){
      char t=Serial.read();
    }
    }