Codigo PID De posicion

// Control PID de posición angular

// Gabriel Peña Peña

// Grupo: ITA-56

// UTEQ

//Librerias

#include <PinChangeInterrupt.h>

#include <PinChangeInterruptBoards.h>

#include <PinChangeInterruptPins.h>

#include <PinChangeInterruptSettings.h>

#include <TimerOne.h>

// PID parametros

double kp = 5, ki = 1.2, kd = 0.05;

double input, output, setpoint;

double iTerm = 0, lastInput = 0, dInput = 0, error = 0;

double outMin = -255, outMax = 255;

double sampleTime = 10; // en ms

volatile long encoderPos = 0;

#define encodPinA1 2 // Pin A encoder del motor

#define encodPinB1 8 // Pin B encoder del motor

#define M1 10 // PWM al módulo controlador de motor L298N

#define M2 11 // PWM al módulo controlador de motor L298N

void setup(void)

{

pinMode(encodPinA1, INPUT); // Codificador de cuadratura de entrada A

pinMode(encodPinB1, INPUT); // Codificador de cuadratura de entrada B

pinMode(led, OUTPUT);

attachPCINT(digitalPinToPCINT(encodPinA1), encoder, FALLING); // Actualizar la posición del codificador

TCCR2B = TCCR2B & 0b11111000 | 1; // Establece 31KH PWM

Timer1.initialize(sampleTime * 1000); // Configura el temporizador 1

Timer1.attachInterrupt(Compute);

Serial.begin(115200);

}

void Compute()

{

setpoint = analogRead(0) * 20; // Posición de ajuste

input = encoderPos; // Datos de codificador

error = setpoint - input;

iTerm += ki * error * sampleTime;

if (iTerm > outMax) iTerm = outMax;

else if (iTerm < outMin) iTerm = outMin;

dInput = (input - lastInput) / sampleTime;

output = kp * error + iTerm - kd * dInput; // Calcula la salida del PID

if (output > outMax) output = outMax; // limita la salida

else if (output < outMin) output = outMin;

lastInput = input;

pwmOut(output);

}

void pwmOut(int out) {

if (out > 0) {

analogWrite(M1, out); // Motor de accionamiento CW

analogWrite(M2, 0);

}

else {

analogWrite(M1, 0);

analogWrite(M2, abs(out)); // Motor de accionamiento CW

}

}

...