62 lines
1.3 KiB
C++
Executable File
62 lines
1.3 KiB
C++
Executable File
#include <Arduino.h>
|
|
#include "esp32-hal-gpio.h"
|
|
#include "mm.h" /* Archivos header ubicados en include/ */
|
|
|
|
void m1handler();
|
|
void m2handler();
|
|
|
|
int m1_rpm = 0;
|
|
int m2_rpm = 0;
|
|
volatile int m1_pulse_cnt = 0;
|
|
volatile int m2_pulse_cnt = 0;
|
|
|
|
void m1handler() {
|
|
if (digitalRead(C2))
|
|
m1_pulse_cnt++;
|
|
else m1_pulse_cnt--;
|
|
}
|
|
void m2handler() {
|
|
if (digitalRead(B2))
|
|
m2_pulse_cnt++;
|
|
else m2_pulse_cnt--;
|
|
}
|
|
|
|
void setup() {
|
|
Serial.begin(115200);
|
|
pinMode(C1, INPUT);
|
|
pinMode(C2, INPUT);
|
|
pinMode(B1, INPUT);
|
|
pinMode(B2, INPUT);
|
|
pinMode(2, OUTPUT);
|
|
pinMode(M1FW, OUTPUT);
|
|
pinMode(M1BW, OUTPUT);
|
|
pinMode(M2FW, OUTPUT);
|
|
pinMode(M2BW, OUTPUT);
|
|
attachInterrupt(C1, m1handler, RISING);
|
|
attachInterrupt(B1, m2handler, RISING);
|
|
}
|
|
|
|
void loop() {
|
|
Serial.print("M1: ");
|
|
Serial.print(m1_pulse_cnt);
|
|
Serial.print(" ");
|
|
Serial.print(m2_pulse_cnt);
|
|
delay(10);
|
|
|
|
/* Un giro de la rueda da 7 pulsos
|
|
* El radio de el reductor es de 1:30
|
|
* Un giro de la rueda serían 210 pulsos*/
|
|
if(m1_pulse_cnt < 200 && m1_pulse_cnt > -210) {
|
|
analogWrite(M1FW, 90);
|
|
} else {
|
|
analogWrite(M1FW, 0);
|
|
}
|
|
|
|
if(m2_pulse_cnt < 200 && m2_pulse_cnt > -210) {
|
|
analogWrite(M1FW, 90);
|
|
} else {
|
|
analogWrite(M1FW, 0);
|
|
}
|
|
Serial.println("");
|
|
}
|