// Observador de Estados Motor DC
// Carlos Mauro Ferrer Riquett
/* Ecuación del observador:

  xek_1 = A*xek+B*uk+L*(yk-C*xek)

*/

// Definición de variables
int entrada = A0; // Entrada de referencia
int corriente = A1;
int velocidad = A2;
int salida = 3; // Acciónd de control
int Ts = 250; // Periodo de muestreo
double uk, yk1, yk2, xk1_1, xk2_1, xk1, xk2; // Variables del sistema real
double xek_1, xek_2, xek1, xek2, yek1, yek2; // Variables del sistema estimado


void sensores(){
  uk = (analogRead(entrada) / 1023.0) * 5; // Escalizado de señal a ref 5
  yk1 = (analogRead(corriente) / 1023.0) * 0.45; //
  yk2 = (analogRead(corriente) / 1023.0) * 15.0; //
}

void setup() {
  Serial.begin(9600);
}

void loop() {

sensores();

 // Motor estimado con observador
   xek_1 = -0.003449 * xek1 - 0.009171 * xek2 + 0.1181 * uk -0.0091 * (yk1 - (1.0 * xek1)); // Ecuación 1
  xek_2 = 0.3057 * xek1 + 0.8124 * xek2 + 0.5207 * uk +0.8090 * (yk2 - (1.0 * xek2)); // Ecuación 2

  yek1 = xek1; // Corriente estimada
  yek2 = xek2; // Velocidad estimada
  
  xek1 = xek_1; // Actualización de señal de retardo 1
  xek2 = xek_2; // Actualización de señal de retardo 2


  // Referencia vs Salida
  Serial.print(yk1, 2);
  Serial.print(" ");
  Serial.print(yk2, 2);
  Serial.print(" ");
  Serial.print(yek1, 2);
  Serial.print(" ");
  Serial.print(yek2, 2);
  Serial.println(" ");
  delay(Ts);

}