control PID para helice vertical

control_propeller.ino

#include <NewPing.h>

#define TRIGGER_PIN  12
#define ECHO_PIN     11
#define MAX_DISTANCE 200 

#define PWM_PIN 9

/* ELIMINACION DELTAS EN LA DERIVADA */
//#define DERIVATIVE_MEASUREMENT //Si esta definido, no se considera el efecto de modificar el setPoint \
                                  para calcular el termino de la derivada. Esto elimina el efecto \
                                  "derivative Kick" ante un escalon del setPoint

/* PERIODO SAMPLEO */
#define Ts_ms 50.0  //en milisegundos
#define Ts (Ts_ms/1e3) //en segundos

/* CONSTANTES PID */
#define _Kp 0.65
#define _Ki 0.5
#define _Kd 0.1

float Kp = _Kp;
float Ki = _Ki * Ts;
float Kd = _Kd / Ts;

/* LIMITES LECTURA SENSOR */
#define SENSOR_READ_MIN 12
#define SENSOR_READ_MAX 86

/* LIMITES VELOCIDAD GIRO HELICE */
//Duty de PWM para lograr las velocidades
#define MOTOR_SPEED_MIN 150
#define MOTOR_SPEED_MAX 250

/* PARA CONTROL DE DELAY */
unsigned long lastReadyTime;

/* ENTRADA DE LA PLANTA */
unsigned int prevPIDoutput = 0;

/* LECTURA DEL SENSOR */
int sensorRead = 12;

/* ENTRADA POR PUERTO SERIE */
String uartData;

/* SET POINT - POSICION DESEADA */
int setPoint = 10;

/* ESTADO DE MOTOR */
bool motor_on = true;

/*
   iAmReady:
   Devuelve true:
   1) La primera vez que se llama.
   2) Si sucedieron mas de Ts milisegundos desde la ultima vez que devolvio true.
*/
bool iAmReady();

int PID_cycle(int setPoint, int sensorRead);

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);


void setup() {
  Serial.begin(9600);
  pinMode(PWM_PIN, OUTPUT);

  //Inicializacion motor helice
  analogWrite(PWM_PIN, 40);
  delay(1000);
  analogWrite(PWM_PIN, 100);
  delay(1000);
  analogWrite(PWM_PIN, 150);
  delay(1000);
  printConsts();
}

void loop() 
{
  
  //OBTENER SET POINT
  if(Serial.available()){
    uartData = Serial.readStringUntil('\n');
    parseInput();
    printConsts();

    if(setPoint==0 || setPoint>86) //apagar el sistema
    {
      motor_on = false;
      Serial.println("Motor apagado");
    } 
    else if(!motor_on) 
    {
      motor_on = true;
      Serial.println("Motor encendido");
    }
  }

  if( !motor_on )
  {
    analogWrite(PWM_PIN, 100);
    return;
  }
  
  //ESPERAR QUE HAYAN SUCEDIDO TS MILISEGUNDOS
  while(!iAmReady()){}

  //OBTENER SALIDA DE LA PLANTA
  sensorRead = sonar.ping_cm();
  
  //CALCULAR LA ENTRADA A LA PLANTA Y MANDAR POR PWM 
  int plantInput = PID_cycle(setPoint, sensorRead);
  
  analogWrite(PWM_PIN, plantInput);
}


int PID_cycle(int setPoint, int sensorRead)
{
  static float prevSensorRead = 0;
  static float prevError = 0;
  static float integralTerm = MOTOR_SPEED_MIN;
  float proportionalTerm;
  float derivativeTerm;
  
  //Si la lectura del sensor esta fuera de rango, usar la anterior
  if ( sensorRead < SENSOR_READ_MIN || sensorRead > SENSOR_READ_MAX ) { sensorRead = prevSensorRead; }



  float error = setPoint - sensorRead;

  /******************************/
  /* Calculo de los 3 terminos: */
  /******************************/
  //(P)
  proportionalTerm = Kp * error;
  //(D)
#ifdef DERIVATIVE_MEASUREMENT
  derivativeTerm = Kd * (-sensorRead - (-prevSensorRead)); //no considera setPoint para evitar "Derivative Kick"
#else
  derivativeTerm = Kd * (error - prevError);
#endif
  //(I)
  integralTerm += Ki * error;
  /******************************/
  
  prevError = error;
  int output = proportionalTerm + integralTerm + derivativeTerm;
  if(output < MOTOR_SPEED_MIN) output = MOTOR_SPEED_MIN;
  if(output > MOTOR_SPEED_MAX) output = MOTOR_SPEED_MAX;


  //backup de valores para la proxima iteracion
  prevSensorRead = sensorRead;
  return output;
}

bool iAmReady()
{
  static bool firstTime = true;
  if (firstTime) //Primera vez que se llama
  {
    lastReadyTime = millis();
    firstTime = false;
    return true;  
  }
  unsigned long currentTime = millis();
  if(currentTime >= lastReadyTime + Ts_ms) //Sucedieron mas de Ts milisegundos desde la ultima vez que devolvio true
  {
    lastReadyTime = currentTime;
    return true;  
  }
  return false;
}


void parseInput()
{
  const char * str = uartData.c_str();
  
  switch(str[0]){
    case 'p': //kp
      Kp = atof(str+1);
      break;
    case 'i': //ki
      Ki = atof(str+1) * Ts; 
      break;
    case 'd': //kd
      Kd = atof(str+1) / Ts;
      break;
    default:  //setpoint
      setPoint = uartData.toInt();
  }
}


/********************
  FUNCIONES DE DEBUG
*********************/


void printDiagnostic(int setPoint, int sensorRead, int output)
{
  Serial.print("SP: ");
  Serial.print(setPoint);
  Serial.print(" - IN: ");
  Serial.print(sensorRead);
  Serial.print(" - OUT: ");
  Serial.print(output);
  Serial.print(" n ");
  Serial.print("\n");
}

void printPIDterms(float p, float i, float d, float error){
  Serial.print("P: ");
  Serial.print(p);
  Serial.print(" - I: ");
  Serial.print(i);
  Serial.print(" - D: ");
  Serial.print(d);
  Serial.print(" - error: ");
  Serial.print(error);
  Serial.print("\n");
  Serial.print("\n");
}


int updateFakeSensorRead(int prevFakeSensorRead)
{
  static int count = 1;
  static int offset = 1;

  if(!(++count % 20)){
    offset = -offset; //cambia el signo
  }

  return prevFakeSensorRead + offset;
}

void printConsts()
{
  Serial.print("SP: ");
  Serial.print(setPoint);
  Serial.print(" KP: ");
  Serial.print(Kp);
  Serial.print(" KI: ");
  Serial.print(Ki/Ts);
  Serial.print(" KD: ");
  Serial.print(Kd*Ts);
  Serial.println(" ");
}