#include <math.

h>

/* Declaración de pines */

const int pinPotMotor = A0;
const int pinPotReferencia = A1;
const int pinIzq = 7;
const int pinDer = 8;
const int pinPWM = 9;
// Definición del pin al que se conecta el potenciómetro del motor
// Definición del pin al que se conecta el potenciómetro de referencia
// Definición del pin que hace girar el motor a la izquierda
// Definición del pin que hace girar el motor a la derecha
// Definición del pin que genera el PWM

/* Declaración de variables enteras */

int anguloMot; // Variable para manejo del ángulo del motor. Varía entre 0 y 1024
int anguloRef; // Variable para manejo del ángulo de referencia. Varía entre 0 y 1024

int PWM; // PWM que se aplica al motor. Varía entre 0 y 255

int error; // Variable para el cálculo del error actualizado

int errorAnterior; // Buffer para almacenar el error anterior, se usa para integrar y derivar

/* Declaración de variables reales */

float integral; // Variable para el cálculo de la integral

float derivada; // Variable para el cálculo de la derivada

float errorP; // Variable para el cálculo de un control proporcional

float errorI; // Variable para el cálculo de un control integral
float errorD; // Variable para el cálculo de un control derivativo

float PID; // Controlador Proporcional Integral Derivativo

/* Declaración de constantes reales */

const float Kp = 1; // Constante para el control proporcional

const float Ki = 0; // Constante para el control integral
const float Kd = 0; // Constante para el control derivativo

const float t = 2; // Constante de tiempo aplicable al cálculo de la integral y la derivada

void setup()
{
pinMode (pinPWM, OUTPUT); // Asiagnación del pin PWM como pin de salida
pinMode (pinIzq, OUTPUT); // Asiagnación del pin para rotación izquierda como pin de salida
pinMode (pinDer, OUTPUT); // Asiagnación del pin para rotación derecha como pin de salida
}
void loop()
{
anguloMot = analogRead(pinPotMotor); // Lectura del ángulo del motor

anguloRef = analogRead(pinPotReferencia); // Lectura del ángulo de referencia

error = anguloRef - anguloMot; // Cálculo del error

integral = integral + ((errorAnterior + error) / 2) * t; // Cálculo de la Integral del error

derivada = (error - errorAnterior) / t; // Cálculo de la Derivada del error

errorP = Kp * error; // Controlador Proporcional

errorI = Ki * integral; // Controlador Integral

errorD = Kd * derivada; // Controlador Derivativo

PID = errorP + errorI + errorD; // Controlador PID

if ( PID < 0 ) // Selección de la rotación del motor

{
digitalWrite (pinIzq, LOW); // Rotación Izquierda desactivada
digitalWrite (pinDer, HIGH); // Rotación Derecha activada
}
else
{
digitalWrite (pinDer, LOW); // Rotación Derecha desactivada
digitalWrite (pinIzq, HIGH); // Rotación Izquierda activada
}

PWM = abs (PID); // Se asegura que el PWM siempre sea positivo

if (PID > 255) PID = 255; // Se asegura que el PWM no sea mayor a 255

analogWrite(pinPWM, PWM); // PWM sobre el motor

errorAnterior = error; // Se almacena el error empleado para ser

// usado en la integral y en la derivada
}