395 lines
14 KiB
Arduino
395 lines
14 KiB
Arduino
#include "freertos/FreeRTOS.h"
|
||
#include "freertos/semphr.h"
|
||
#include "freertos/task.h"
|
||
#include <Arduino.h>
|
||
#include <cmath>
|
||
|
||
// ==========================================
|
||
// PROTOCOLO UART
|
||
// ==========================================
|
||
//
|
||
// RX (RPi → ESP32):
|
||
// M:s1,f1,s2,f2,s3,f3,s4,f4\n
|
||
// s = signo → -1 | 0 | 1
|
||
// f = fracción 0.000–1.000 de VEL_MAX_TICKS del motor
|
||
// S:\n → Stop inmediato
|
||
// C:\n → Iniciar autocalibración
|
||
//
|
||
// TX (ESP32 → RPi):
|
||
// T:t1,t2,t3,t4\n → Odometría acumulada (ticks con signo)
|
||
// K:v1,v2,v3,v4\n → VEL_MAX_TICKS de cada motor (ticks/s) — tras calibrar
|
||
// D:...\n → Debug (solo por USB/Serial)
|
||
//
|
||
// ==========================================
|
||
|
||
// ─── FLAGS ────────────────────────────────────────────────────────────────────
|
||
volatile bool iniciar_calibracion = false;
|
||
volatile bool en_calibracion = false;
|
||
|
||
// ─── PINES ────────────────────────────────────────────────────────────────────
|
||
const int PIN_RX_RPI = 44, PIN_TX_RPI = 43;
|
||
const int PIN_STBY_1 = 40, PIN_STBY_2 = 7;
|
||
|
||
const int PIN_M1_IN1 = 15, PIN_M1_IN2 = 16, PIN_M1_PWM = 17, PIN_ENC1 = 9;
|
||
const int PIN_M2_IN1 = 6, PIN_M2_IN2 = 5, PIN_M2_PWM = 4, PIN_ENC2 = 10;
|
||
const int PIN_M3_IN1 = 41, PIN_M3_IN2 = 42, PIN_M3_PWM = 2, PIN_ENC3 = 36;
|
||
const int PIN_M4_IN1 = 39, PIN_M4_IN2 = 38, PIN_M4_PWM = 37, PIN_ENC4 = 35;
|
||
|
||
const int FREC_PWM = 1000, RES_PWM = 8;
|
||
|
||
const int IN1[] = {PIN_M1_IN1, PIN_M2_IN1, PIN_M3_IN1, PIN_M4_IN1};
|
||
const int IN2[] = {PIN_M1_IN2, PIN_M2_IN2, PIN_M3_IN2, PIN_M4_IN2};
|
||
const int PWM[] = {PIN_M1_PWM, PIN_M2_PWM, PIN_M3_PWM, PIN_M4_PWM};
|
||
const int ENC[] = {PIN_ENC1, PIN_ENC2, PIN_ENC3, PIN_ENC4};
|
||
|
||
// ─── ENCODERS ─────────────────────────────────────────────────────────────────
|
||
volatile unsigned long pulsos[4] = {0, 0, 0, 0};
|
||
void IRAM_ATTR isr_E1() { pulsos[0]++; }
|
||
void IRAM_ATTR isr_E2() { pulsos[1]++; }
|
||
void IRAM_ATTR isr_E3() { pulsos[2]++; }
|
||
void IRAM_ATTR isr_E4() { pulsos[3]++; }
|
||
|
||
long global_ticks[4] = {0, 0, 0, 0};
|
||
SemaphoreHandle_t mutexDatos;
|
||
|
||
// ─── VELOCIDAD MÁXIMA CALIBRADA ───────────────────────────────────────────────
|
||
// En ticks/s. La RPi la pide con C: y la recibe en K:
|
||
float VEL_MAX_TICKS[4] = {200.0f, 200.0f, 200.0f, 200.0f};
|
||
|
||
// Banda muerta por motor = 3% de VEL_MAX (se recalcula tras calibrar)
|
||
float BANDA_MUERTA[4] = {6.0f, 6.0f, 6.0f, 6.0f};
|
||
|
||
// ==========================================
|
||
// CLASE PID
|
||
// ==========================================
|
||
class PID {
|
||
public:
|
||
float kp, ki, kff;
|
||
float error_acumulado = 0.0f;
|
||
float setpoint = 0.0f; // ticks/s, con signo
|
||
|
||
PID(float _kp, float _ki, float _kff) : kp(_kp), ki(_ki), kff(_kff) {}
|
||
|
||
// vel_actual: ticks/s con signo
|
||
// banda : umbral bajo el cual el error se ignora (ticks/s)
|
||
int calcularPWM(float vel_actual, float dt, float banda) {
|
||
if (fabsf(setpoint) < 1.0f) {
|
||
error_acumulado = 0.0f;
|
||
return 0;
|
||
}
|
||
|
||
float error = setpoint - vel_actual;
|
||
|
||
// Banda muerta solo en régimen estacionario (motor ya girando)
|
||
if (fabsf(error) < banda && fabsf(vel_actual) > 5.0f)
|
||
error = 0.0f;
|
||
|
||
error_acumulado += error * dt;
|
||
float lim = 200.0f / (ki + 0.0001f);
|
||
error_acumulado = constrain(error_acumulado, -lim, lim);
|
||
|
||
float salida = kp * error
|
||
+ ki * error_acumulado
|
||
+ copysignf(fabsf(setpoint) * kff, setpoint);
|
||
|
||
return (int)constrain(salida, -255.0f, 255.0f);
|
||
}
|
||
|
||
void reset() { error_acumulado = 0.0f; setpoint = 0.0f; }
|
||
};
|
||
|
||
PID pid[4] = {
|
||
PID(2.0f, 1.5f, 0.0f), // M1 FL
|
||
PID(2.0f, 1.5f, 0.0f), // M2 FR
|
||
PID(2.0f, 1.5f, 0.0f), // M3 RL
|
||
PID(2.0f, 1.5f, 0.0f), // M4 RR
|
||
};
|
||
|
||
// ─── MOTOR ────────────────────────────────────────────────────────────────────
|
||
void moverMotor(int idx, int val) {
|
||
if (abs(val) < 15) {
|
||
digitalWrite(IN1[idx], LOW);
|
||
digitalWrite(IN2[idx], LOW);
|
||
ledcWrite(PWM[idx], 0);
|
||
return;
|
||
}
|
||
if (val > 0) { digitalWrite(IN1[idx], HIGH); digitalWrite(IN2[idx], LOW); }
|
||
else { digitalWrite(IN1[idx], LOW); digitalWrite(IN2[idx], HIGH); val = -val; }
|
||
ledcWrite(PWM[idx], val);
|
||
}
|
||
|
||
void pararTodos() {
|
||
for (int i = 0; i < 4; i++) moverMotor(i, 0);
|
||
}
|
||
|
||
// ─── LECTURA ATÓMICA DE ENCODERS ──────────────────────────────────────────────
|
||
// portENTER_CRITICAL_ISR bloquea ambos cores en ESP32 (a diferencia de noInterrupts)
|
||
static portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED;
|
||
|
||
inline void leerPulsos(unsigned long dest[4]) {
|
||
portENTER_CRITICAL(&mux);
|
||
for (int i = 0; i < 4; i++) dest[i] = pulsos[i];
|
||
portEXIT_CRITICAL(&mux);
|
||
}
|
||
|
||
// ==========================================
|
||
// TAREA: CONTROL DE MOTORES (CORE 1)
|
||
// ==========================================
|
||
void tareaControlMotores(void *pvParameters) {
|
||
unsigned long p_ant[4] = {0, 0, 0, 0};
|
||
static float v_f[4] = {0, 0, 0, 0};
|
||
const float alpha = 0.35f;
|
||
const float dt = 0.05f;
|
||
const TickType_t xFrec = pdMS_TO_TICKS(50);
|
||
TickType_t xUltimo = xTaskGetTickCount();
|
||
|
||
for (;;) {
|
||
|
||
// ────────────────────────────────────────────────────────────────────────
|
||
// AUTOCALIBRACIÓN
|
||
// ────────────────────────────────────────────────────────────────────────
|
||
if (iniciar_calibracion) {
|
||
en_calibracion = true;
|
||
for (int i = 0; i < 4; i++) pid[i].reset();
|
||
pararTodos();
|
||
vTaskDelay(pdMS_TO_TICKS(500));
|
||
|
||
unsigned long p_base[4];
|
||
leerPulsos(p_base);
|
||
for (int i = 0; i < 4; i++) p_ant[i] = p_base[i];
|
||
|
||
// — Paso 1: fricción estática ————————————————————————————————————————
|
||
int friccion[4] = {0, 0, 0, 0};
|
||
for (int p = 20; p <= 130; p += 2) {
|
||
for (int i = 0; i < 4; i++)
|
||
if (friccion[i] == 0) {
|
||
digitalWrite(IN1[i], LOW);
|
||
digitalWrite(IN2[i], HIGH);
|
||
ledcWrite(PWM[i], p);
|
||
}
|
||
vTaskDelay(pdMS_TO_TICKS(60));
|
||
|
||
unsigned long p_act[4];
|
||
leerPulsos(p_act);
|
||
bool todos = true;
|
||
for (int i = 0; i < 4; i++) {
|
||
if (friccion[i] == 0) {
|
||
if ((p_act[i] - p_ant[i]) > 1) friccion[i] = p;
|
||
else todos = false;
|
||
}
|
||
p_ant[i] = p_act[i];
|
||
}
|
||
if (todos) break;
|
||
}
|
||
|
||
// — Paso 2: velocidad máxima (kff y VEL_MAX_TICKS) ——————————————————
|
||
const int TEST_PWM = 200;
|
||
for (int i = 0; i < 4; i++) {
|
||
digitalWrite(IN1[i], LOW);
|
||
digitalWrite(IN2[i], HIGH);
|
||
ledcWrite(PWM[i], TEST_PWM);
|
||
}
|
||
vTaskDelay(pdMS_TO_TICKS(800)); // estabilización
|
||
|
||
unsigned long p_ss1[4]; leerPulsos(p_ss1);
|
||
vTaskDelay(pdMS_TO_TICKS(300));
|
||
unsigned long p_ss2[4]; leerPulsos(p_ss2);
|
||
|
||
for (int i = 0; i < 4; i++) {
|
||
float vel_medida = (float)(p_ss2[i] - p_ss1[i]) / 0.3f; // ticks/s
|
||
|
||
// VEL_MAX extrapolada a PWM 255
|
||
VEL_MAX_TICKS[i] = (vel_medida > 10.0f)
|
||
? vel_medida * (255.0f / (float)(TEST_PWM - friccion[i]))
|
||
: 200.0f;
|
||
VEL_MAX_TICKS[i] = constrain(VEL_MAX_TICKS[i], 50.0f, 1000.0f);
|
||
|
||
// kff: PWM por ticks/s
|
||
pid[i].kff = (vel_medida > 10.0f)
|
||
? (float)(TEST_PWM - friccion[i]) / vel_medida
|
||
: 2.0f;
|
||
pid[i].kff = constrain(pid[i].kff, 0.1f, 8.0f);
|
||
|
||
// Banda muerta = 3% de VEL_MAX
|
||
BANDA_MUERTA[i] = VEL_MAX_TICKS[i] * 0.03f;
|
||
|
||
pid[i].reset();
|
||
}
|
||
|
||
pararTodos();
|
||
|
||
// Reporte por USB
|
||
Serial.println("=== CALIBRACION COMPLETA ===");
|
||
for (int i = 0; i < 4; i++)
|
||
Serial.printf("M%d fric=%d vel_max=%.1f kff=%.3f banda=%.1f\n",
|
||
i+1, friccion[i], VEL_MAX_TICKS[i], pid[i].kff, BANDA_MUERTA[i]);
|
||
|
||
// ── Trama K: → RPi para que actualice su VEL_MAX ──────────────────────
|
||
// Tanto por USB como por Serial1 (UART RPi)
|
||
char buf[80];
|
||
snprintf(buf, sizeof(buf), "K:%.1f,%.1f,%.1f,%.1f\n",
|
||
VEL_MAX_TICKS[0], VEL_MAX_TICKS[1],
|
||
VEL_MAX_TICKS[2], VEL_MAX_TICKS[3]);
|
||
Serial.print(buf);
|
||
Serial1.print(buf);
|
||
|
||
leerPulsos(p_ant);
|
||
iniciar_calibracion = false;
|
||
en_calibracion = false;
|
||
}
|
||
// ────────────────────────────────────────────────────────────────────────
|
||
|
||
// Leer encoders
|
||
unsigned long p_act[4];
|
||
leerPulsos(p_act);
|
||
|
||
unsigned long d_p[4];
|
||
for (int i = 0; i < 4; i++) {
|
||
d_p[i] = p_act[i] - p_ant[i];
|
||
p_ant[i] = p_act[i];
|
||
}
|
||
|
||
// Dirección del setpoint para acumular odometría con signo
|
||
int d_f[4];
|
||
for (int i = 0; i < 4; i++)
|
||
d_f[i] = (pid[i].setpoint > 1.0f) ? 1 :
|
||
(pid[i].setpoint < -1.0f) ? -1 : 0;
|
||
|
||
if (xSemaphoreTake(mutexDatos, pdMS_TO_TICKS(5)) == pdTRUE) {
|
||
for (int i = 0; i < 4; i++)
|
||
global_ticks[i] += (long)d_p[i] * d_f[i];
|
||
xSemaphoreGive(mutexDatos);
|
||
}
|
||
|
||
// Velocidad filtrada con EMA
|
||
for (int i = 0; i < 4; i++) {
|
||
float v_cruda = ((float)d_p[i] * (float)d_f[i]) / dt;
|
||
v_f[i] = alpha * v_cruda + (1.0f - alpha) * v_f[i];
|
||
}
|
||
|
||
// PID y actuación
|
||
for (int i = 0; i < 4; i++)
|
||
moverMotor(i, pid[i].calcularPWM(v_f[i], dt, BANDA_MUERTA[i]));
|
||
|
||
// Debug cada 500ms (10 ciclos × 50ms)
|
||
static int dbg = 0;
|
||
if (++dbg >= 10) {
|
||
dbg = 0;
|
||
Serial.printf("D:SP=%.0f,%.0f,%.0f,%.0f V=%.0f,%.0f,%.0f,%.0f\n",
|
||
pid[0].setpoint, pid[1].setpoint, pid[2].setpoint, pid[3].setpoint,
|
||
v_f[0], v_f[1], v_f[2], v_f[3]);
|
||
}
|
||
|
||
vTaskDelayUntil(&xUltimo, xFrec);
|
||
}
|
||
}
|
||
|
||
// ==========================================
|
||
// PARSEO DE TRAMA (Core 0)
|
||
// ==========================================
|
||
void procesarTrama(const String &t) {
|
||
if (t.length() < 2) return;
|
||
char cmd = t.charAt(0);
|
||
|
||
if (cmd == 'C') { iniciar_calibracion = true; return; }
|
||
if (cmd == 'S') { for (int i = 0; i < 4; i++) pid[i].reset(); return; }
|
||
|
||
if (cmd == 'M' && t.length() > 2) {
|
||
// Formato: M:s1,f1,s2,f2,s3,f3,s4,f4
|
||
// s = -1, 0, o 1 | f = 0.000–1.000
|
||
int s[4]; float f[4];
|
||
if (sscanf(t.c_str() + 2,
|
||
"%d,%f,%d,%f,%d,%f,%d,%f",
|
||
&s[0], &f[0], &s[1], &f[1],
|
||
&s[2], &f[2], &s[3], &f[3]) != 8)
|
||
return;
|
||
|
||
for (int i = 0; i < 4; i++) {
|
||
f[i] = constrain(f[i], 0.0f, 1.0f);
|
||
// Setpoint en ticks/s con signo aplicado directamente
|
||
pid[i].setpoint = (float)s[i] * f[i] * VEL_MAX_TICKS[i];
|
||
}
|
||
return;
|
||
}
|
||
}
|
||
|
||
// ==========================================
|
||
// TAREA: COMUNICACIÓN (Core 0)
|
||
// ==========================================
|
||
void tareaComunicacion(void *pvParameters) {
|
||
String tUSB = "", tRPI = "";
|
||
unsigned long uEnvio = 0;
|
||
unsigned long ultimaVezRecibido = millis();
|
||
|
||
for (;;) {
|
||
// Telemetría cada 40ms
|
||
if (millis() - uEnvio >= 40) {
|
||
uEnvio = millis();
|
||
long tk[4];
|
||
if (xSemaphoreTake(mutexDatos, pdMS_TO_TICKS(5)) == pdTRUE) {
|
||
for (int i = 0; i < 4; i++) tk[i] = global_ticks[i];
|
||
xSemaphoreGive(mutexDatos);
|
||
}
|
||
char buf[64];
|
||
snprintf(buf, sizeof(buf), "T:%ld,%ld,%ld,%ld\n", tk[0], tk[1], tk[2], tk[3]);
|
||
Serial.print(buf);
|
||
Serial1.print(buf);
|
||
}
|
||
|
||
// Lectura USB
|
||
while (Serial.available()) {
|
||
char c = Serial.read();
|
||
if (c == '\n') {
|
||
tUSB.trim();
|
||
if (tUSB.length()) { procesarTrama(tUSB); ultimaVezRecibido = millis(); }
|
||
tUSB = "";
|
||
} else { tUSB += c; }
|
||
}
|
||
|
||
// Lectura RPi
|
||
while (Serial1.available()) {
|
||
char c = Serial1.read();
|
||
if (c == '\n') {
|
||
tRPI.trim();
|
||
if (tRPI.length()) { procesarTrama(tRPI); ultimaVezRecibido = millis(); }
|
||
tRPI = "";
|
||
} else { tRPI += c; }
|
||
}
|
||
|
||
// Freno de emergencia (ignora calibración en curso)
|
||
// 800ms da margen para lags normales del SO de la RPi
|
||
if (!en_calibracion && millis() - ultimaVezRecibido > 800)
|
||
for (int i = 0; i < 4; i++) pid[i].reset();
|
||
|
||
vTaskDelay(pdMS_TO_TICKS(2));
|
||
}
|
||
}
|
||
|
||
// ==========================================
|
||
// SETUP
|
||
// ==========================================
|
||
void setup() {
|
||
Serial.begin(115200);
|
||
Serial1.begin(921600, SERIAL_8N1, PIN_RX_RPI, PIN_TX_RPI);
|
||
|
||
pinMode(PIN_STBY_1, OUTPUT); digitalWrite(PIN_STBY_1, HIGH);
|
||
pinMode(PIN_STBY_2, OUTPUT); digitalWrite(PIN_STBY_2, HIGH);
|
||
|
||
for (int i = 0; i < 4; i++) {
|
||
pinMode(IN1[i], OUTPUT);
|
||
pinMode(IN2[i], OUTPUT);
|
||
ledcAttach(PWM[i], FREC_PWM, RES_PWM);
|
||
pinMode(ENC[i], INPUT);
|
||
}
|
||
|
||
attachInterrupt(digitalPinToInterrupt(ENC[0]), isr_E1, RISING);
|
||
attachInterrupt(digitalPinToInterrupt(ENC[1]), isr_E2, RISING);
|
||
attachInterrupt(digitalPinToInterrupt(ENC[2]), isr_E3, RISING);
|
||
attachInterrupt(digitalPinToInterrupt(ENC[3]), isr_E4, RISING);
|
||
|
||
mutexDatos = xSemaphoreCreateMutex();
|
||
xTaskCreatePinnedToCore(tareaControlMotores, "Ctrl", 4096, NULL, 2, NULL, 1);
|
||
xTaskCreatePinnedToCore(tareaComunicacion, "Com", 4096, NULL, 1, NULL, 0);
|
||
}
|
||
|
||
void loop() { vTaskDelay(pdMS_TO_TICKS(10)); } |