PÉNDULO INVERTIDO

Análisis Dinámico:

Este artículo presenta un estudio sobre el control de un péndulo invertido en un carro móvil
sobre una viga. El péndulo invertido, un sistema clásico ampliamente utilizado en laboratorios
de control, presenta desafíos significativos debido a su falta de estabilidad. En este trabajo, se
aborda el problema mediante el diseño e implementación de un Control Proporcional-Integral-
Derivativo (PID) discreto. Se analiza el sistema utilizando técnicas de control no lineal y se
linealiza el sistema para derivar las ecuaciones de movimiento. Se discuten los resultados
obtenidos, que muestran la eficacia del controlador PID discreto en estabilizar el péndulo
invertido durante el movimiento del carro sobre la viga. Este trabajo proporciona una
contribución significativa al campo del control de sistemas no lineales, especialmente en
aplicaciones que requieren estabilidad dinámica.

Asumiendo fricción=0:

Por sumatoria de fuerzas en el eje X para el carrito tenemos:

∑ F x =m ẍ
F−R x =M ẍ

Por sumatoria de fuerzas en el eje X para el péndulo tenemos:

 ∑ F x =ma x
R x =m( ẍ +at cos θ−ar sin θ)
2
R x =m ẍ +mL θ̈ cos θ−mL θ̇ sinθ

Reemplazamos R x en la ecuación del carrito:

2
F=M ẍ +m ẍ+ mL θ̈ cos θ−mL θ̇ sin θ
2
F=( M +m ) ẍ +mL θ̈ cos θ−mL θ̇ sinθ …(1)
Por sumatoria de fuerzas en el eje perpendicular al péndulo (analizado desde el sistema de
referencia no inercial con origen en el carrito):

Ry

Rx

Donde a f es la aceración ficticia.

a f = ẍ
'
F 1=m ẍ cos θ
'
F 2=m ẍ sin θ
Tenemos:
 R y sin θ−R x cos θ−mg sin θ+ m ẍ cos θ=−mL θ̈

Por sumatoria de momentos:

L R y sinθ−L R x cos θ=I θ̈

I θ̈
R y sin θ−R x cos θ=
L
Reemplazando las dos últimas ecuaciones:

I θ̈
−mg sin θ=−mL θ̈−m ẍ cos θ
L
2
I θ̈−Lmg sin θ=−L m θ̈−Lm ẍ cos θ
( I +m L2 ) θ̈−mgL sin θ=−mL ẍ cos θ …(2)
Por linealización tenemos:

sin θ ≈ θ
cos θ ≈ 1
2
θ̇ ≈ 0
Sustituimos en (1) y (2):

F=( M +m ) ẍ +mL θ̈ …(3)

( I +m L2 ) θ̈−mgLθ=−mL ẍ …(4)

Función de transferencia:

Tomamos la transformada de Laplace de nuestras ecuaciones:

2 2
F=( M +m ) X s + mL Θ s …(5)

( I +m L2 ) Θ s 2−mgL Θ=−mLX s2 …(6)

Primero obtenemos la función de transferencia con Θ como salida y F como entrada.

Para ello eliminamos X despejándolo primero de (6):

X=
[ − ( I + m L2 ) g
mL
+ 2 Θ
s ]
Sustituimos en (5):

F=( M +m )
[ −( I +m L2) g
mL
2
]
+ 2 Θ s +mL Θ s
s
2
 Θ 1
=

[ ]
F −( I +m L2 ) g 2 2
( M +m ) + 2 s +mL s
mL s

Θ 1
=
F 2 ( M +m ) ( I + m L2 ) 2
mL s − s + ( M +m ) g
mL
1
Θ mL
=

( )
F ( M + m ) ( I +m L2 ) 2 ( M + m )
1− s+ g
m 2 L2 mL

−mL
Θ ( M +m ) ( I + m L2 )−m2 L2
=
F 2 ( M + m) mLg
s−
( M +m ) ( I +m L2 )−m2 L2
Para la retroalimentación tenemos:

mL
Θ ( M +m ) ( I + m L2 )−m2 L2
=
F 2 ( M + m) mLg
s−
( M +m ) ( I +m L2 )−m2 L2
mL
Θ q
=
F ( M +m ) mLg
s2−
q

Ahora obtenemos la función de transferencia con X como salida y F como entrada.

Para ello eliminamos Θ :

( I +m L2 ) Θ s 2−mgL Θ=−mLX s2
2
−mLX s
Θ=
( I +m L2 ) s 2−mgL
Sustituyendo:
2 2 4
2 m L Xs
F=( M +m ) X s −
( I +m L2) s2−mgL
 X 1
=
F 2
m L s
2 4
( M + m) s2−
( I + m L2 ) s 2−mgL
X ( I +m L2) s2−mgL
=
F ( M + m) ( ( I +m L2) s2 −mgL ) s2 −m2 L2 s 4

X ( I +m L2) s2 −mgL
=
F ( M + m) ( I +m L2 ) s 4 −( M + m ) mgL s 2−m2 L2 s 4

X ( I +m L2 ) s2 −mgL
=
F [ ( M + m ) ( I +m L2 ) −m2 L2 ] s 4− ( M + m) mgL s2

( I + m L2 ) 2 mgL
s−
X ( M +m ) ( I + m L )−m L
2 2 2
( M +m ) ( I + m L2 )−m2 L2
=
F 4 ( M +m ) mgL 2
s− s
( M +m ) ( I + m L )−m L
2 2 2

q=( M +m ) ( I +m L2 )−m2 L2

( I +m L2 )mgL 2
s−
X q q
=
F 4 ( M +m ) mgL 2
s− s
q

Simulación error estado estable y comprobación de polos:

Para la posición x, realizaremos el siguiente código en Mathlab:

G=tf([2 0 -1],[3 0 19.62 0 0]);

E_R=1/(1+G);
p=pole(G);
t=linspace(0,15,150);
r=zeros(length(t),1);
r(t>=2)=1;
lsim(E_R,r,t)

Donde obtenemos la siguiente gráfica que determina el error de estado estable:

Además, tenemos los siguientes polos:

Para el ángulo θ, realizaremos el siguiente código en Mathlab:

G=tf(1,[-3 0 19.62]);
E_R=1/(1+G);
p=pole(G);
t=linspace(0,15,150);
r=zeros(length(t),1);
r(t>=2)=1;
lsim(E_R,r,t)

Donde obtenemos la siguiente gráfica que determina el error de estado estable:

Además, tenemos los siguientes polos:

LUGAR A RAICES EN GRÁFICAS:

PARA LAS FUNCIONES DE:

Θ 1
G= =
F −3 s +19.62
2
 2
X 2 s −1
G= =
F 3 s 4 +19.62 s2
ESPACIO DE ESTADOS:

De las ecuaciones:

F=( M +m ) ẍ +mL θ̈ …(3)

( I +m L2 ) θ̈−mgLθ=−mL ẍ …(4)
De (4):

mgLθ −mL ẍ
θ̈= 2
I +m L

Reemplazando en (3):

F=( M +m ) ẍ +mL θ̈

F=( M +m ) ẍ +mL
( mgLθ−mL ẍ
I+m L
2 )
( )
2 2 2 2
m L −m g L θ
ẍ ( M +m )− 2
= 2
+F
I+m L I +m L

−m2 g L2 F
ẍ= θ+

( ) m 2 L2
2 2
( I + m L2 ) M +m− m L M +m−
I + m L2 I + m L2

2 2 2
−m g L I +m L
ẍ= θ+ F
( m+ M ) ( I +m L ) −m L
2 2 2
( I +m L ) ( M + m )−m2 L2
2

2 2 2
−m g L I +m L
ẍ= 2
θ+ F
( M + m ) I + Mm L ( M +m ) I + Mm L2
De (4):

mgLθ−( I +m L2 ) θ̈
ẍ=
mL

Reemplazando en (3):

F=( M +m ) ẍ +mL θ̈

F=( M +m ) ( mgLθ−( I + m L2 ) θ̈
mL )
+mL θ̈

( M +m ) ( I + m L2 )
F=( M +m ) gθ− θ̈+ mL θ̈
mL

( m+ M ) gmL mL
θ̈= θ− F
( m+ M ) ( I + m L )−m L
2 2 2
( m+ M ) ( I +m L2) −m2 L2

mgL (M + m) mL
θ̈= 2
θ− F
( M +m ) I + Mm L ( M +m ) I + Mm L2

Definimos nuestras variables de estado:

x 1=x , x 2= ẋ , x 3=θ , x 4 =θ̇ ,

[]
x
X = ẋ
θ
θ̇

Derivamos nuestras variables de estado:

x˙1= ẋ , x˙2= ẍ , x˙3=θ̇ , x˙4 =θ̈ ,

[]
ẋ
Ẋ = ẍ
θ̇
θ̈
 ẋ= ẋ

2 2 2
−m g L I +m L
ẍ= 2
θ+ F
( M + m ) I + Mm L ( M +m ) I + Mm L2

θ̇=θ̇

mgL (M + m) mL
θ̈= 2
θ− F
( M +m ) I + Mm L ( M +m ) I + Mm L2

[ ][] [ ][]
x1 x ẋ 1 ẋ
x2 ẋ 2 = ẍ
Construimos nuestra ecuación matricial: X = = ẋ , Ẋ=
x3 θ ẋ 3 θ̇
x4 θ̇ x˙ 4 θ̈

[ ][ ] [ ]
0 1 0 0 0

[]
ẋ 0 0 −m2 g L2 I +m L
2
0 x
ẍ ( M +m ) I + Mm L2 ẋ ( M +m ) I + Mm L2
= + F
θ̇ 0 0 0 1 θ 0
θ̈ mgL (M + m) θ̇ −mL
0 0 0
( M +m ) I + Mm L2 ( M +m ) I + Mm L2

El cual tiene la forma:

Ẋ =AX + BF

Dicha ecuación corresponde al sistema no retroalimentado:

Para los valores que tenemos:

Por la realimentación se genera:

Ẋ =B ( r −KX )+ AX

Ẋ =( A−BK ) X +rB

'
Ẋ =A X +rB

'
A = A−BK

K=¿

Para hallar la ecuación característica de lazo cerrado:

det ( sI −A ' )=0

 det ( sI −A +BK )=0

Al resolver esta ecuación obtendremos las raíces o polos del sistema. De este modo al

asumir las raíces del sistema podemos obtener una ecuación característica que luego

puede ser comparada con esta ecuación para obtener los valores de K.

Este procedimiento puede simplificarse al uso de la función place() de Matlab.

Código de Matlab:

clc
clear
syms s
syms K1 K2 K3 K4
%G=tf([1],[1 0 -1])
%ng=s+3
%dg=expand(s*(s+1)*(s+2)*(s+4))
%G=tf(-1,[-3 0 19.62])
%p=pole(G)
%rlocus(G)
m=0.04;
M=0.043;
L=0.9;
I=1/3*m*L^2
g=9.81;
b=0;

a=m*L/((M+m)*(I+m*L^2)-m^2*L^2)
G=tf(a,[1 0 a])
figure
rlocus(G)
q=(M+m)*(I+m*L^2)-m^2*L^2;
G2=tf([(I+m*L^2)/q 0 -m*g*L/q],[1 0 -(M+m)*m*g*L/q 0 0])
%----------Espacio de estados:

q = I*(M+m)+M*m*L^2; %denominator for the A and B matrices

a23=(m^2*g*L^2)/q
a44=m*g*L*(M+m)/q

A = [0 1 0 0;
0 0 -(m^2*g*L^2)/q 0;
0 0 0 1;
0 0 m*g*L*(M+m)/q 0];
B = [ 0;
(I+m*L^2)/q;
0;
-m*L/q];
C = [1 0 0 0;
0 0 1 0];
D = [0;
0];

Im=[1 0 0 0;
0 1 0 0;
0 0 1 0;
0 0 0 1]
K=[K1 K2 K3 K4]
polcar=s*Im-A+B*K
determinante=det(polcar)
%simplify(determinante)

Deseamos que dos de los polos se encuentren muy alejados del origen para que no
tengan mucho efecto en el resultado y los otros dos sean polos complejos simétricos con
respecto al eje real.
Suponemos los siguientes polos:
%polos deseados:
p=[-100,-200,complex(-5,2),complex(-5,-2)]
k_place=place(A,B,p)

El programa nos arroja:

Que corresponde a:

K= [−3760.2−1353−5977.8−1643.4 ]

Para la simulación en Simulink tomamos en cuenta que el ángulo inicial es de 0°, por lo

que le daremos una entrada Impulso adicional:

La entrada impulso que se ha escogido tiene las siguientes características:

Tenemos por lo tanto una entrada Step con un valor de 1, del cual su derivada (impulso)

entra al sistema como una fuerza. Por lo tanto:

δ=F

∫ δ dt=∫ F dt
 m
10 N . s=10 kg =∫ F dt
s

Lo cual se puede interpretar de la siguiente manera: Dado que el tiempo de aplicación

de la fuerza es muy pequeña por la misma naturaleza de la función impulso, ella se

podría aproximar a aplicar una fuerza de 20N durante 0.5s en la base del carro.

Obtenemos la siguiente salida en el Scope

La curva azul representa la velocidad lineal

La curva verde representa la velocidad angular

Observamos con mayor detalle las otras dos variables:

La curva amarilla representa la posición y la curva naranja representa el ángulo.
Recordemos que, por inercia al aplicar una fuerza positiva en el carro, el péndulo tiende
a irse hacia el lado negativo (generando ángulos negativos), esta es la razón por la que
las curvas de los ángulos son negativas al generarse el impulso externo.

Relación Voltaje Fuerza:

De las características del motor tenemos:

Tenemos que a 12V obtenemos un torque de 1.4 kg.cm o lo que es lo mismo:

13.734 N.cm
Aproximando la función de transferencia de un motor a:
τ =bv
Donde b es la constante del motor y v es el voltaje aplicado, tendríamos:
Dado que la polea tiene 1cm de radio:
F=Rτ
F=(1cm)(13.734 N . cm)
F=13.734 N
Esta es la fuerza producida sobre el carro cuando al motor se le aplica un voltaje de 12V
De la ecuación del motor:
τ =bv
F=cv
Donde c es otra constante a calcular:
F
c=
v
Dado que conocemos la fuerza obtenida cuando se aplica 12V reemplazamos:
13.734 N
c=
12V
c=1.144
Entonces:
F=1.144 v
Observamos como varía la fuerza aplicada sobre el carro:
Podemos observar que luego del impulso, el controlador en respuesta genera una fuerza
en sentido opuesto que llega a un máximo de 110N aproximadamente, esto, según la
relación entre fuerza y voltaje que hemos obtenido, equivaldría a aproximadamente 96V
aplicados al motor. Como sabemos esto no es posible, por lo que tendremos que
cambiar el valor de la perturbación externa:

Esto corresponde a 1N.s lo que es equivalente a golpear el carro de la base con una
fuerza de 10N en un impacto de duración de 0.1s
Observamos la salida:
Graficamos la fuerza aplicada sobre el carro con respecto al tiempo:

Se genera una fuerza máxima de 11N lo que equivale a aplicar 9.62v en el motor.
Esto significa que el motor es capaz de volver a estabilizar el sistema ante un impulso
de 1N.s

Respuesta ante una perturbación en el ángulo del péndulo:

Suponiendo que ahora perturbamos el ángulo del péndulo de la siguiente manera:
Obtenemos la siguiente respuesta:

Se grafican tanto la posición como el ángulo. Se observa que el sistema se vuelve

estable luego de aproximadamente 0.3s

Código de Arduino:
#include <math.h>
#include <Wire.h>
#include <KalmanFilter.h>

KalmanFilter kalmanX(0.001, 0.003, 0.03);

KalmanFilter kalmanY(0.001, 0.003, 0.03);
KalmanFilter kalmanZ(0.001, 0.003, 0.03);

unsigned long interval_inc = 1000; // LED 2 parpadea cada 5 segundos

unsigned long previousMicros_inc = 0;

float RateRoll, RatePitch, RateYaw;

float RateCalibrationRoll, RateCalibrationPitch, RateCalibrationYaw;
int RateCalibrationNumber;
float AccX, AccY, AccZ;
float AngleRoll, AnglePitch, AngleYaw;
uint32_t LoopTimer;
//float KalmanAngleRoll=0, KalmanUncertaintyAngleRoll=2*2;
//float KalmanAnglePitch=0, KalmanUncertaintyAnglePitch=2*2;
float KalmanAngleYaw=0, KalmanUncertaintyAngleYaw=2*2;
float KalmanAngleV=0;
float KalmanAnglePast=0;

// Definición de pines
const int pinA = 3;
const int pinB = 2;
const int IN1=4;
const int IN2=5;
const int ENA=9;
const int finalCarrera=6;
int v=130;

//variable de inicio
bool inicio=1;
bool inicio2=0;
bool inicio3=0;
bool proceso=0;

// Variables
volatile int position = 0;
volatile int velocity = 0;

// Variables auxiliares
volatile boolean stateA;
volatile boolean stateB;
volatile boolean lastStateA;

unsigned long lastTime = 0;

unsigned long lastTimeMotor=0;
unsigned long lastTimeBoton=0;
const unsigned long interval_p = 1; // Intervalo de tiempo entre
lecturas en milisegundos
//const unsigned long interval_v = 100;
//const unsigned long intervalMotor=200;
const unsigned long intervalBoton=1;

float lastPosition=0;
float currentPosition=0;
float currentVelocity=0;
float pi=M_PI;

void gyro_signals(void) {
Wire.beginTransmission(0x68);
Wire.write(0x1A);
Wire.write(0x05);
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1C);
Wire.write(0x10);
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0x68,6);
int16_t AccXLSB = Wire.read() << 8 | Wire.read();
int16_t AccYLSB = Wire.read() << 8 | Wire.read();
int16_t AccZLSB = Wire.read() << 8 | Wire.read();
Wire.beginTransmission(0x68);
Wire.write(0x1B);
Wire.write(0x8);
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x43);
Wire.endTransmission();
Wire.requestFrom(0x68,6);
int16_t GyroX=Wire.read()<<8 | Wire.read();
int16_t GyroY=Wire.read()<<8 | Wire.read();
int16_t GyroZ=Wire.read()<<8 | Wire.read();
RateRoll=(float)GyroX/65.5;
RatePitch=(float)GyroY/65.5;
RateYaw=(float)GyroZ/65.5;
AccX=(float)AccXLSB/4096;
AccY=(float)AccYLSB/4096;
AccZ=(float)AccZLSB/4096;
AngleRoll=atan(AccY/sqrt(AccX*AccX+AccZ*AccZ))*1/(3.142/180);
AnglePitch=-atan(AccX/sqrt(AccY*AccY+AccZ*AccZ))*1/(3.142/180);
AngleYaw=-atan(AccY/sqrt(AccZ*AccZ+AccX*AccX))*1/(3.142/180);
}
void setup() {
//TCCR1B = TCCR1B & B11111000 | B00000001; //cambiamos frecuencia PWM a
30kHz
TCCR1B = TCCR1B & B11111000 | B00000010;
// Configuración de pines
pinMode(pinA, INPUT);
pinMode(pinB, INPUT);
 pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(ENA, OUTPUT);
pinMode(finalCarrera, INPUT_PULLUP);
// Configuración de interrupciones
attachInterrupt(digitalPinToInterrupt(pinA), updateEncoder, CHANGE);
//attachInterrupt(digitalPinToInterrupt(pinB), updateEncoder, CHANGE);

// Inicialización de variables auxiliares

stateA = digitalRead(pinA);
stateB = digitalRead(pinB);
lastStateA = stateA;

//Inicio del puerto serie

Serial.begin(9600);
pinMode(13, OUTPUT);
digitalWrite(13, HIGH);
Wire.setClock(400000);
Wire.begin();
delay(250);
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0x00);
Wire.endTransmission();
for (RateCalibrationNumber=0; RateCalibrationNumber<2000;
RateCalibrationNumber ++) {
gyro_signals();
//RateCalibrationRoll+=RateRoll;
//RateCalibrationPitch+=RatePitch;
RateCalibrationYaw+=RateYaw;
delay(1);
}
//RateCalibrationRoll/=2000;
//RateCalibrationPitch/=2000;
RateCalibrationYaw/=2000;
LoopTimer=micros();
}
void loop() {
// Obtener el tiempo actual
unsigned long currentMicros = micros();
unsigned long currentTime = millis();
bool estadoFC = digitalRead(finalCarrera);

if (currentMicros - previousMicros_inc >= interval_inc) {

previousMicros_inc = currentMicros; // Actualizar el tiempo anterior
gyro_signals();
//RateRoll-=RateCalibrationRoll;
//RatePitch-=RateCalibrationPitch;
RateYaw-=RateCalibrationYaw;
 KalmanAngleYaw = kalmanZ.update(AngleYaw, RateYaw);
KalmanAngleV=pi/180*(KalmanAngleYaw-
KalmanAnglePast)*1000000/interval_inc;
KalmanAnglePast=KalmanAngleYaw;
//kalRoll = kalmanX.update(accRoll, gyr.XAxis);
//---------------------------------
//Serial.print("Roll Angle [°] ");
//Serial.print(KalmanAngleRoll);
//--------------------------
//Serial.print(" Sin ");
//Serial.print(AngleYaw);
//Serial.print(" vel ");
Serial.print(KalmanAngleV);
Serial.print("\t");
//-----------------------

//Serial.print(" Pitch Angle [°] ");

//Serial.print(" Yaw Angle [°] ");
//Serial.println(KalmanAnglePitch);
Serial.println(KalmanAngleYaw);

}
if (currentTime - lastTimeBoton >= intervalBoton) {
// Actualizar última vez
lastTimeBoton = currentTime;
if (!estadoFC){
inicio=0;
inicio2=1;
}
if (inicio){
calibrarPos();
}
else if(inicio2){
detener();
position=-1833;
delay(1);
inicio2=0;
inicio3=1;
//goto0(position);
}
else if(inicio3){
if (position<0 ){
int error=-position;
//int vel=map(error,1833,0,160,40);
vel=map(error,1833,0,200,100);
goto0(vel);
 }
else{
detener();
inicio3=0;
proceso=1;
}
}
else if(proceso && KalmanAngleYaw>-20 && KalmanAngleYaw<20){
if (currentPosition>-240 && currentPosition <240){
float errorAngulo=KalmanAngleYaw-0.2;
float errorVangulo=KalmanAngleV;
float errorPos=currentPosition;
float errorVel=currentVelocity;
float K1=180;
float K2=56;
float K3=200;
float K4=164;
int
velocidad=int(K1*errorAngulo+K2*errorVangulo+K3*errorPos+K4*errorVel);
int vMotor=0;
int min=18;
int med=40;
if (velocidad<=med && velocidad>min){
vMotor=med;
}
else if(velocidad>=-med && velocidad<-min){
vMotor=-med;
}
else if(velocidad>-min && velocidad<min){
vMotor=0;
}
else if(velocidad>=255){
vMotor=255;
}
else if(velocidad<=-255){
vMotor=-255;
}
else{
vMotor=velocidad;
}
controlMotor(vMotor);

}
else{
detener();
}
}
else{
 detener();
}

if (currentTime - lastTime >= interval_p) {

// Actualizar última vez
lastTime = currentTime;

// Obtener posición y velocidad actual

noInterrupts(); // Desactivar interrupciones para evitar errores
currentPosition = float(position)/11*pi/20*(30*2/(2*pi))*1;

currentVelocity = (currentPosition-lastPosition)*1000/interval_p;
lastPosition=currentPosition;

interrupts(); // Activar interrupciones nuevamente

// Imprimir posición y velocidad en el puerto serie

//Serial.print("Posicion: ");
//-Serial.print(currentPosition);
//Serial.print(" mm");
//-Serial.print("\t");
//Serial.print("\tVelocidad: ");
//-Serial.println(currentVelocity);
//Serial.print(" unidades por segundo");
}
}

void calibrarPos(){
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
analogWrite(ENA, v);
}
void detener(){
analogWrite(ENA,0);
}
void goto0(int vel){

digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
analogWrite(ENA,vel);

void controlMotor(int velo){

if (velo>=0){
digitalWrite(IN1, HIGH);
 digitalWrite(IN2, LOW);
analogWrite(ENA,velo);
}
else{
int velo2=-velo;
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
analogWrite(ENA,velo2);
}

void updateEncoder() {
stateA = digitalRead(pinA);
stateB = digitalRead(pinB);

if (stateB != stateA) {
position++;
} else {
position--;
}

//velocity = position - lastPosition;

//lastPosition = position;

lastStateA = stateA;
}

Fotos de la Implementación:
Link del video del funcionamiento:
https://drive.google.com/drive/folders/1aOxF4HTBp2vBs-rZwdKbs_yDRrPA4iaZ?
usp=sharing