REPUBLICA DEL ECUADOR

UNIVERSIDAD TÉCNICA DEL NORTE

FACULTAD DE INGENIERÍA EN CIENCIAS APLICADAS
Mecatrónica

FORMATO PARA LA PRESENTACIÓN DEL PROYECTO

1. Título del proyecto

Implementación de un robot móvil con un brazo robótico.

2. Datos generales

Equipo de Trabajo

Participantes Apellidos y Nombres Correo Institucional

Estudiante Coordinador Minango Rojas César Augusto caminangor@utn.edu.ec

Estudiante Coordinador Villa Carcelén Axel Fernando afvillac@utn.edu.ec

Cachiguango Lema Elvis
Estudiante Participante eacachiguangol@utn.edu.ec
Andrés
Estudiante Participante Fonte Paisano Jhon Smith Jsfontep@utn.edu.ec

Ipiales Tuquerez Anthony

Estudiante Participante
Estudiante Participante
Estudiante Participante
Estudiante Participante
aiipialest@utn.edu.ec
Israel
Narváez Narváez Byron
bsnarvaezn@utn.edu.ec
Stalyn
Paguay Cuaspud Jefferson
japaguayc@utn.edu.ec
Alexander
Pozo Benavides Marlon Alexis mapozobutn.edu.ec

Pupiales Pupiales Jefferson

Estudiante Participante japupialesp@utn.edu.ec
Andrés
Estudiante Participante Terán Rosales José Isacc jiteranr@utn.edu.ec

3. Problema
El uso de manipuladores cada vez es más común en las industrias sin embargo con el constante
desarrollo de la tecnología se requiere que los procesos a realizar sean automatizados (Guanín,
2015). La fusión entre un robot móvil y un brazo robótico es algo muy interesante debido a que
este podría llevar a cabo funciones de desplazarse tomando un objeto del punto A y llevándolo
hacia el punto B sin necesidad de la intervención de un humano.

4. Justificación
Los robots móviles pueden efectuar tareas de gran complejidad en ambientes de trabajo poco
estructurados y que representen un peligro para la vida humana (Casper & Murphy, 2003). El
impacto de los robots móviles en la vida cotidiana y en entornos profesionales es innegable, lo
que subraya la necesidad de abordar cuestiones éticas cruciales. Por ejemplo, en el ámbito
médico, la autonomía de los robots para tomar decisiones durante procedimientos quirúrgicos
plantea interrogantes éticos sobre la responsabilidad y la toma de decisiones. La utilización de
brazos acoplados a estos robots permite ampliar el rango de operaciones que se pueda realizar,
entre ellas pueden ser la desactivación de bombas, traslado de cargas, remoción de escombros
y exploración de terrenos difíciles (García, y otros, 2017). El objetivo de los robots no es sustituir

Página 1 de 18
REPUBLICA DEL ECUADOR

UNIVERSIDAD TÉCNICA DEL NORTE

FACULTAD DE INGENIERÍA EN CIENCIAS APLICADAS
Mecatrónica

a los operarios sino optimizar el trabajo, además de garantizar la seguridad e impulsar el

rendimiento de cada uno de los procesos. Esta nueva generación de robots está diseñada para
trabajar en la industria convirtiéndose asi en una herramienta adecuada para automatizar
procesos propios de un entorno de fabricación (Robotnik, 2023). Cada día se aprende mas
sobre este tipo de modelos es por ello que el desarrollo de este tipo de proyectos lo tornan
interesante ya que ayuda a satisfacer necesidades, además ayuda a poner en practica lo
revisado en clase.

5. Objetivos

Objetivo General:
• Construir un robot móvil que contenga un brazo robótico encima.

Objetivos Específicos:
• Rediseñar y remplazar piezas que no sean funcionales tanto del robot móvil como del
brazo robótico.
• Programar el robot móvil para que cumpla una trayectoria asignada en Python.
• Programar y controlar el brazo robótico cumpliendo el criterio de Denavit Hartenberg.
• Validar funcionalidad.

6. Metodología
Investigación Documental: Esta investigación se realizó con el propósito de profundizar y
comparar las conceptualizaciones a detalle de los robots móviles, se tomó como base diferentes
fuentes de información tales como libros, revistas y páginas web, las mismas que ayudaron a
comprender sobre un robot móvil y un brazo robótico esto fue aplicado en su rediseño y su
programación.
Simulación: La etapa de simulación del sistema se lo realizó en tinkercad ya que esta
herramienta nos ayuda a crear circuitos antes de su implementación con los cuales pudimos
simular los movimientos de los motores, articulaciones y servomotores.
Ensamblaje: Una vez obtenidas las piezas y los componentes a utilizar se llevó a cabo la
implementación de estos, la programación del microcontrolador permite el movimiento del carro
y el brazo.

7. Cronograma y Recursos

Secuencia lógica de actividades. Especificar el talento humano, recursos económicos y

materiales.

8. Resultados

A través del desarrollo de este proyecto, se adquirió un entendimiento práctico y didáctico

del funcionamiento integral de un brazo robótico integrado en un robot móvil. La ejecución
de esta iniciativa permitió el fortalecimiento significativo de destrezas, habilidades técnicas
y conocimientos en el ámbito de la robótica industrial. El prototipo resultante, habilitado
con capacidades de reconocimiento y seguimiento facial, presenta una gama diversa de
posibles aplicaciones, tal como señala el trabajo de titulación realizado por Portalanza et
al. (2016) en su estudio sobre brazos robóticos industriales en procesos de soldadura,
destacando la importancia de los robots industriales como elementos clave en la
Página 2 de 18
REPUBLICA DEL ECUADOR

UNIVERSIDAD TÉCNICA DEL NORTE

FACULTAD DE INGENIERÍA EN CIENCIAS APLICADAS
Mecatrónica

progresión y desarrollo de las empresas en el sector manufacturero.

Las potenciales aplicaciones de este prototipo abarcan diversos ámbitos, incluyendo la
asistencia médica. Por ejemplo, en cirugía guiada por imágenes, la capacidad de
seguimiento facial permitiría al brazo robótico seguir los movimientos del cirujano,
asegurando la precisión de las herramientas quirúrgicas, tal como sugieren varios
estudios sobre la integración de robots en entornos médicos avanzados (Johnson et al.,
2020). Asimismo, este tipo de robots pueden desempeñar un papel crucial como
asistentes en el hogar o en entornos de atención al cliente, ofreciendo respuestas
personalizadas y mejorando la interacción y satisfacción del usuario.
La versatilidad y las múltiples aplicaciones potenciales de este robot móvil con brazo
robótico destacan su importancia en diversos campos, desde el industrial hasta el médico
y de atención al cliente, subrayando su relevancia en la evolución tecnológica y su
potencial para mejorar la eficiencia y la experiencia del usuario en distintos entornos.

Robot Móvil Diferencial

Un robot diferencial es un tipo de robot que se caracteriza por su capacidad para moverse
y maniobrar mediante la variación de la velocidad y dirección de sus ruedas, maneja un
principio de diferenciales similar a los vehículos de tracción trasera. Un robot diferencial
consta típicamente de dos ruedas colocadas en ejes paralelos y un tercer soporte como
una rueda de apoyo Fuente especificada no válida. .

Se obtuvo mediante el docente el robot móvil diferencial a modificar, la cual se puede

observar en la figura 1. La misma que cuenta con los siguientes elementos:
- 2 llantas
- 2 motores 150 rpm a 9v
- 1 rueda loca
- ESP32
- Puente HL298N
- Alimentación

Figura 1. Robot móvil diferencial otorgado por el docente

Página 3 de 18
REPUBLICA DEL ECUADOR

UNIVERSIDAD TÉCNICA DEL NORTE

FACULTAD DE INGENIERÍA EN CIENCIAS APLICADAS
Mecatrónica

Tomando en cuenta el objetivo principal del proyecto el cual es la integración del brazo
robótico al robot móvil, se realiza modificaciones al diseño del robot móvil inicial, ya que
no contaba con una base ni diseño adecuado para la integración del brazo robótico, por lo
cual en la figura 2 se observa el diseño seleccionado por el grupo de trabajo.

Figura 2. Modificación de robot móvil

Configuración de robot móvil

Con los materiales obtenidos se procede a realizar el ensamble y a establecer la
resolución de cada uno de los motores encoder de 150rpm utilizando la tarjeta ESP32,
con el programa en Arduino desarrollado en clase se señala el inicio del motor para
comprobar la resolución que se da al girar una vuelta completa en cada uno de los
motores.

Figura 3. Definición de variables para recepción de resolución del encoder.

En el código mostrado en la figura 3 se definen las señales de entradas de los enconder

tanto derecho como izquierdo. Además, se nombran variables que determina el estado
actual de los enconder, obteniendo:
Rueda derecha (RR)= 298
Rueda izquierda (RL)= 300.

Una vez obtenidas las resoluciones se procede a implementar el código en el software de

Arduino el cual está configurado para mostrar los grados rotados por la rueda, para ello se
ha usado la siguiente fórmula:
Página 4 de 18
REPUBLICA DEL ECUADOR

UNIVERSIDAD TÉCNICA DEL NORTE

FACULTAD DE INGENIERÍA EN CIENCIAS APLICADAS
Mecatrónica

Rueda derecha
Rueda izquierda
Con esta fórmula se transforma los valores obtenidos a grados. En la figura 4 se observa
la incorporación de los valores de la resolución de cada uno de los encoder anteriormente
obtenidos. Además, se utiliza la función void computePosicionInDegrees, la cual se
encarga de transformar los valores a grados.

Figura 4. Código para imprimir grados rotados por los encoder derecha e izquierda.

Una vez ejecutado el programa, se comprueba que el valor mostrado en el monitor serial
al momento de dar un giro completo de la rueda sea de +-360° según su sentido de giro.

Velocidad en RPM
Dentro de la programación del robot móvil se puede imprimir los valores que arroja cada
uno de los encoder, como en este caso las RPM. Los enconder permiten controlar la
velocidad angular en base a la detección de la posición y dirección de giro, con estas
variables es posible controlar el movimiento del robot móvil.
para el uso de los encoder se hace el uso de los pines que permitan PWM, que mediante
la modificación del ciclo de trabajo se consigue controlar cantidad de la energía que se
envía todo esto en un motor DC, la alimentación variara de manera proporcional con
respecto a la parte alta y baja.

Página 5 de 18
REPUBLICA DEL ECUADOR

UNIVERSIDAD TÉCNICA DEL NORTE

FACULTAD DE INGENIERÍA EN CIENCIAS APLICADAS
Mecatrónica

Figura 5. Código para determinar las RPM

En la figura 5 se observa el código con el cual se determina el movimiento de las ruedas,

dependiendo al valor ingresado al serial.
Un punto elemental para obtener una buena recepción de datos es determinar la
velocidad angular que está dada por la siguiente formula:
2𝜋 ∗ 1000 ∗ 𝑛
𝜔=
𝑡∗𝑅
Donde:
R = Resolución
n = cuentas- encoder
t = tiempo

2𝜋∗1000
Por lo cual la constante a determinar es la siguiente: 𝑅
(constValue), de cada uno de
los encoder, valores reflejados en la figura 6.

Figura 6. Valores constValue derecho e izquierdo.

Una vez obtenidos los valores necesarios para determinar la velocidad angular tanto en la
Página 6 de 18
REPUBLICA DEL ECUADOR

UNIVERSIDAD TÉCNICA DEL NORTE

FACULTAD DE INGENIERÍA EN CIENCIAS APLICADAS
Mecatrónica

rueda derecha como en la izquierda, se colocan estos datos en el archivo de Arduino lazo
abierto, con el cual se procede a realizar las determinadas pruebas de funcionamiento.
Controlador PID
Control de lazo abierto
En la representación del sistema y seguimiento de referencia que se presenta en el plano,
es posible identificar las señales apreciadas en la figura 7.

Figura 7. Respuesta de lazo abierto.

Como se observa en la figura 7 las señales tienen un cierto ruido de interferencia, esto se
debe a que el sistema aún no se encuentra optimizado, pero la reacción de estas
corresponde al desplazamiento original.

Corrección de giro con controlador PID

Se procede a colocar valores obtenidos de Python en la programación de Arduino con la
finalidad de realizar un control de lazo cerrado. Se realizan cambios de valores con lo que
la señal se estabilice como se observa en la figura 8.

Figura 8. Modificación de datos en Arduino.

Optimización de datos generados

Página 7 de 18
REPUBLICA DEL ECUADOR

UNIVERSIDAD TÉCNICA DEL NORTE

FACULTAD DE INGENIERÍA EN CIENCIAS APLICADAS
Mecatrónica

Se procede a importar el documento PSO, donde se regulan los datos con la finalidad de
corregir los ruidos presentados anteriormente. Una vez finalizada las correcciones se
ejecuta el programa firstOrderSystem donde se procede a llamar a RSO, dando asi la
siguiente gráfica (Figura 9).

Figura 9. Control en lazo abierto optimizado por PSO.

Página 8 de 18
REPUBLICA DEL ECUADOR

UNIVERSIDAD TÉCNICA DEL NORTE

FACULTAD DE INGENIERÍA EN CIENCIAS APLICADAS
Mecatrónica

Brazo Robótico

Cinemática directa
En el problema cinemático directo se conocen los valores de sus coordenadas
articulares y busca determinar la posición del efector final, esto se refiere que la
solución encontrada será única. En generar, un robot de n grados de libertad está
formado por n eslabones unidos a n articulaciones, a cada eslabón se le puede asociar
un sistema de referencia y utilizando las transformaciones homogéneas es posible
representar las rotaciones y traslaciones relativas entre los distintos eslabones que
componen el brazo robot (García, y otros, 2017).

Página 9 de 18
REPUBLICA DEL ECUADOR

UNIVERSIDAD TÉCNICA DEL NORTE

FACULTAD DE INGENIERÍA EN CIENCIAS APLICADAS
Mecatrónica

Estas 4 transformaciones básicas consisten en una sucesión de rotaciones y

traslaciones que permiten relacionar el sistema de referencia del elemento i-1 con el
sistema del elemento i.
1. Rotación sobre el eje 𝑧𝑖−1 un ángulo 𝜃𝑖.
2. Traslación a lo largo de 𝑧𝑖−1 una distancia 𝑑𝑖.
3. Traslación a lo largo de 𝑥𝑖 una distancia 𝑎𝑖.
4. Rotación sobre el eje 𝑥𝑖 un ángulo 𝛼𝑖.

Se debe tener en cuenta que el producto de las matrices no es conmutativo, por lo tanto,
se deben ejecutar en el siguiente orden:
i-1Ai =𝑅𝑜𝑡𝑧(𝜃𝑖) ∗ 𝑇(0,0, 𝑑𝑖) ∗ 𝑇(𝑎𝑖, 0,0) ∗ 𝑅𝑜𝑡𝑥(𝖺𝑖)

Debido a que un robot es considerado como una cadena cinemática que se encuentra
conformado por eslabones unidos entre si mediante articulaciones, se puede establecer
un sistema de referencia fijo en la base del robot y de esta manera poder describir de
una mejor manera la localización de cada uno de los eslabones, tomando como
referencia dicho sistema de origen (R. Kryscia, 2021).

En la figura 1 se puede observar la estructura final que debe contener la matriz de

transformación homogénea.

Figura 10. Matriz de Transformación Homogénea.

Fuente: Disponible en Kryscia (2021).

Cinemática inversa
Según R. Maria En el caso de la cinemática inversa se conoce la posición y orientación
del efector final, pero se desconocen las variables articulares. Por lo tanto, es necesario
resolver un conjunto de ecuaciones no lineales, de manera que se implementan
diferentes métodos numéricos para obtener el resultado correcto.

Al momento de utilizar cinemática directa permite tomar el problema de una manera

sistemática tomando las matrices de transformación homogénea, e independiente de la
configuración del robot. Sin embargo, esto no es posible en la cinemática inversa,
siendo el procedimiento para las ecuaciones fuertemente dependiente de la
configuración establecida del robot. Sin embargo, contrastando con la cinemática
inversa, se advierte que el procedimiento para las ecuaciones está fuertemente
influenciado por la configuración especifica previamente establecida del robot.

Página 10 de 18
REPUBLICA DEL ECUADOR

UNIVERSIDAD TÉCNICA DEL NORTE

FACULTAD DE INGENIERÍA EN CIENCIAS APLICADAS
Mecatrónica

La resolución para el problema llega a proporcionar múltiples soluciones, esto debido a

que genera diferentes configuraciones articular con las que obtener la misma
configuración del efector final, de otra forma también pueden generarse soluciones que
no existan debido q posiciones que el brazo no alcance a llegar, o puede dar lugar a
diferentes singularidades. De esta forma, hace más difícil la resolución de problema que
tomando un enfoque directo.

Figura 11. Problema cinemático inverso. a) Solución múltiple, b) codo abajo.

Fuente: Recuperado de Kryscia (2021).

En la figura 2 se puede visualizar de una manera sencilla la existencia de diversas

soluciones para el problema cinemático inverso en un brazo robótico que contas de 2
GDL. Las soluciones 1 y 2 son comúnmente conocidas como codo arriba y codo abajo
respectivamente. De esta manera, la búsqueda de solución se suele utilizar el uso de
técnicas numéricas iterativas como por ejemplo Método de Newton. Sin embargo, el
uso de este proceso resulta en cálculos lentos, por lo que habitualmente en una
implementación real sería demasiado tardío.

Página 11 de 18
REPUBLICA DEL ECUADOR

UNIVERSIDAD TÉCNICA DEL NORTE

FACULTAD DE INGENIERÍA EN CIENCIAS APLICADAS
Mecatrónica

Materiales:
• Impresora 3D
• Computadora
• Filamento PETG • Placa Arduino UNO
• Protoboard.
• Jumpers (Macho – Macho, Macho - Hembra)
• Pulsador
• 3 servomotores Sg90
• 3 servomotores Mg7
• 7 pasadores 2/16
• 4 pernos M4 x 1
• Corte laser
• Placa esp32
• Baterias
• Fuente 1A

Mariz denavit hartenberg obtenida con nuestro sistema de referencia

Articulacion ᴓ d a ǿ
1 q1 L1 0 90
2 q2 0 L2 0
3 q3 0 L3 0
4 q4 0 L4 0
Fuente: Narváez(2023).

Página 12 de 18
REPUBLICA DEL ECUADOR

UNIVERSIDAD TÉCNICA DEL NORTE

FACULTAD DE INGENIERÍA EN CIENCIAS APLICADAS
Mecatrónica

9. Bibliografía

Bibliografía
Casper, J., & Murphy, R. (2003). Robot interactions during the robot-assited urban
search and rescue response at the World Trade Center. IEEE TRANSACTIONS
ON SYSTEMS.
García, J., Medina, I., Martínez, J., García, A., Linares, A., & Porras, C. (2017). Robót
móvil dotado de brazo para contacto con el suelo. Revista iberoamericana de
automática e informática industrial, 174-183.
Guanín, E. (2015). Sistema de control electrónico mediante comandos de voz para un
robot secuencial.
Robotnik. (31 de Marzo de 2023). Robornik:Usos y ventajas de los cobots en la
industria manufacturera. Obtenido de https://robotnik.eu/es/usos-y-ventajas-de-
los-cobots-en-la-industria-manufacturera/
Guanín Eduardo. (2015). Sistema de control electrónico mediante comandos de voz
para un robot secuencial.

Hernández Víctor. (2011). Manufactura de un brazo robótico SCARA.

Elaboración de un prototipo de brazo robótico industrial didáctico automatizado para

proceso de soldadura GMAW. Rigoberto Minda-Valverde, I. I., & Fernando
RicaurteSegovia, A. I. (2021).

Diseño e Implementación de un Brazo Robotico Industrial con 5 Grados de Libertad

Guiado por Kinect Design and Implementation of an Industrial Robot Arm with 5
Degrees of Freedom Guided by Kinect Projeto e implementação de um braço robótico
industrial com 5 graus de liberdade

María, G. (2014, octubre). Modelación y Análisis de la Cinemática Directa e Inversa del

Manipulador Stanford de Seis Grados de Libertad.
Kryscia, R. (2021, mayo). Cinemática Directa del Robot.

María, G. (2014, octubre). Modelación y Análisis de la Cinemática Directa e Inversa del

Manipulador Stanford de Seis Grados de Libertad.

10. Anexos
CODIGO ARUINO PARA ROBOT MOVIL
#include <motorControl.h>

Página 13 de 18
REPUBLICA DEL ECUADOR

UNIVERSIDAD TÉCNICA DEL NORTE

FACULTAD DE INGENIERÍA EN CIENCIAS APLICADAS
Mecatrónica

//----------SAMPLING VARIABLES----------//
unsigned long lastTime = 0;
unsigned long sampleTime = 100;

//***DATA RECEPTION**//
String inputString = "";
bool stringComplete = false;
const char separador = ',';
const int dataLength = 2;
double datos[dataLength];

//***PID CONTROLER**///
motorControl motorR(sampleTime);
motorControl motorL(sampleTime);

//----------LEFT ENGINE----------//
const int C1L = 33; //Encoder signal A input - Yellow colored wire.
const int C2L = 32; //Encoder signal B input - Green colored wire.
volatile int nL = 0;
volatile byte antL = 0;
volatile byte actL = 0;
volatile byte estadoL = 0;

const byte in1 = 26;

const byte in2 = 25;
const byte ena = 27;
double angularspeedLrpm = 0.0;
double wL = 0.0;

//----------RIGHT ENGINE----------//
const int C1R = 34; //Encoder signal B input - Green colored wire.
const int C2R = 35; //Encoder signal A input - Yellow colored wire.
volatile int nR = 0;
volatile byte antR = 0;
volatile byte actR = 0;
volatile byte estadoR = 0;

const byte in3 = 14;

const byte in4 = 12;
const byte enb = 13;
double angularspeedRrpm = 0.0;
double wR = 0.0;

//----------RIGHT-HAND MOTOR DIRECTION OF ROTATION----------//

Página 14 de 18
REPUBLICA DEL ECUADOR

UNIVERSIDAD TÉCNICA DEL NORTE

FACULTAD DE INGENIERÍA EN CIENCIAS APLICADAS
Mecatrónica

void ICACHE_RAM_ATTR encoderR() {

antR = actR;
if (digitalRead(C1R) == 0 && digitalRead(C2R) == 0) estadoR = 0;
if (digitalRead(C1R) == 0 && digitalRead(C2R) == 1) estadoR = 1;
if (digitalRead(C1R) == 1 && digitalRead(C2R) == 0) estadoR = 2;
if (digitalRead(C1R) == 1 && digitalRead(C2R) == 1) estadoR = 3;
actR = estadoR;

//Anti-clockwise
if (antR == 0 && actR == 2) nR++;
if (antR == 1 && actR == 0) nR++;
if (antR == 2 && actR == 3) nR++;
if (antR == 3 && actR == 1) nR++;

//Clockwise
if (antR == 0 && actR == 1) nR--;
if (antR == 1 && actR == 3) nR--;
if (antR == 2 && actR == 0) nR--;
if (antR == 3 && actR == 2) nR--;
}

//----------LEFT-HAND MOTOR DIRECTION OF ROTATION----------//

void ICACHE_RAM_ATTR encoderL() {
antL = actL;
if (digitalRead(C1L) == 0 && digitalRead(C2L) == 0) estadoL = 0;
if (digitalRead(C1L) == 0 && digitalRead(C2L) == 1) estadoL = 1;
if (digitalRead(C1L) == 1 && digitalRead(C2L) == 0) estadoL = 2;
if (digitalRead(C1L) == 1 && digitalRead(C2L) == 1) estadoL = 3;
actL = estadoL;

//Anti-clockwise
if (antL == 0 && actL == 2) nL++;
if (antL == 1 && actL == 0) nL++;
if (antL == 2 && actL == 3) nL++;
if (antL == 3 && actL == 1) nL++;

//Clockwise
if (antL == 0 && actL == 1) nL--;
if (antL == 1 && actL == 3) nL--;
if (antL == 2 && actL == 0) nL--;
if (antL == 3 && actL == 2) nL--;
}

//---------QUADRUPLE PRECISION RESOLUTION--------//

//const int R = 924;
const int RR = 194;
const int RL = 192;

Página 15 de 18
REPUBLICA DEL ECUADOR

UNIVERSIDAD TÉCNICA DEL NORTE

FACULTAD DE INGENIERÍA EN CIENCIAS APLICADAS
Mecatrónica

double PosR = 0.0;

double PosL = 0.0;

//***CONTROL VARIABLE***//
int cvR = 0;
int cvL = 0;

//**PWM VARIABLE**///
const int PWMfreq = 6000;
const int PWMres = 8;

const double constValueR = 1855.67;

const double constValueL = 1875;

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

motorR.setCvLimits(255,180);
motorR.setPvLimits(4062,0);

motorL.setCvLimits(255,180);
motorL.setPvLimits(4085,0);
//----------INPUTS----------//
pinMode(C1R, INPUT);
pinMode(C2R, INPUT);
pinMode(C1L, INPUT);
pinMode(C2L, INPUT);

pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);

digitalWrite(in1, 0);
digitalWrite(in2, 0);
digitalWrite(in3, 0);
digitalWrite(in4, 0);

//----------INTERRUPTIONS----------//
attachInterrupt(C1R, encoderR, CHANGE);
attachInterrupt(C2R, encoderR, CHANGE);
attachInterrupt(C1L, encoderL, CHANGE);
attachInterrupt(C2L, encoderL, CHANGE);

//**PWM**///
ledcSetup(1, PWMfreq, PWMres);
ledcAttachPin(ena, 1);
Página 16 de 18
REPUBLICA DEL ECUADOR

UNIVERSIDAD TÉCNICA DEL NORTE

FACULTAD DE INGENIERÍA EN CIENCIAS APLICADAS
Mecatrónica

ledcSetup(2, PWMfreq, PWMres);

ledcAttachPin (enb, 2);
}

void loop() {
serialEvent();
if (stringComplete) {
for (int i = 0; i < dataLength; i++) {
int index = inputString.indexOf(separador);
datos[i] = inputString.substring(0, index).toFloat();
inputString = inputString.substring(index + 1);
}

cvR = motorR.scaleCv(datos[0]);
cvL = motorL.scaleCv(datos [1]);

if (cvR > 0) clockwise(in1, in2, 1, cvR); else anticlockwise(in1, in2, 1, abs(cvR));

if (cvL > 0) clockwise(in4, in3, 2, cvL); else anticlockwise(in4, in3, 2, abs(cvL));

inputString = "";
stringComplete = false;
}
if (millis() - lastTime >= sampleTime) {
computeW();
wR = motorR.scalePv(wR);
wL = motorL.scalePv(wL);
Serial.println(wR);
Serial.println(wL);
delay(500);
}
}

void computeW(void) {
wR = (constValueR * nR) / ((millis() - lastTime));
wL = (constValueL * nL) / ((millis() - lastTime));
lastTime = millis();
nR = 0;
nL = 0;
}
void serialEvent() {
while (Serial.available()) {
char inChar = (char)Serial.read();
inputString += inChar;
if (inChar == '\n') {
stringComplete = true;

}
}
Página 17 de 18
REPUBLICA DEL ECUADOR

UNIVERSIDAD TÉCNICA DEL NORTE

FACULTAD DE INGENIERÍA EN CIENCIAS APLICADAS
Mecatrónica

void anticlockwise (int pin1, int pin2, int analogPin, int PWM) {
digitalWrite(pin1, HIGH);
digitalWrite(pin2, LOW);
ledcWrite(analogPin, PWM);
}

void clockwise (int pin1, int pin2, int analogPin, int PWM) {
digitalWrite(pin1, LOW);
digitalWrite(pin2, HIGH);
ledcWrite(analogPin, PWM);
}

Página 18 de 18