Mesa auto-nivelable

i_ricson · July 1, 2024, 1:21am

Que tal, estoy desarrollando un proyecto el cual consiste en nivelar una mesa automaticamente por medio de 4 pistones electricos con motores DC controlados con L298N, ademas de un sensor MPU6050 para obtener los angulos de nivelacion, quisiera saber que opinan del siguiente codigo y que mejoras podria incluir o cambiar. Lo que hace es primero mover dos pistones para nivelar el eje X, y despues los otros dos para nivelar el eje Y, pero siento que no es tan eficiente.

#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"

MPU6050 sensor;

// Definiciones de pines
const byte M1IN1 = 2;
const byte M1IN2 = 3;
const byte M2IN3 = 4;
const byte M2IN4 = 5;

const byte M3IN1 = 6;
const byte M3IN2 = 7;
const byte M4IN3 = 8;
const byte M4IN4 = 9;

const byte botonAjuste = 12;
const byte botonReset = 13;

// Valores RAW (sin procesar) del acelerómetro en los ejes x, y, z
int ax, ay, az;

void setup() {
  Serial.begin(57600);    // Iniciando puerto serial
  Wire.begin();           // Iniciando I2C  
  sensor.initialize();    // Iniciando el sensor

  if (sensor.testConnection()) Serial.println("Sensor iniciado correctamente");
  else Serial.println("Error al iniciar el sensor");

  // Configuración de pines de salida
  pinMode(M1IN1, OUTPUT);
  pinMode(M1IN2, OUTPUT);
  pinMode(M2IN3, OUTPUT);
  pinMode(M2IN4, OUTPUT);
  pinMode(M3IN1, OUTPUT);
  pinMode(M3IN2, OUTPUT);
  pinMode(M4IN3, OUTPUT);
  pinMode(M4IN4, OUTPUT);
}

void loop() {
  // Leer las aceleraciones 
  sensor.getAcceleration(&ax, &ay, &az);
  // Calcular los ángulos de inclinación:
  float angleX = atan(ax / sqrt(pow(ay, 2) + pow(az, 2))) * (180.0 / 3.14);
  float angleY = atan(ay / sqrt(pow(ax, 2) + pow(az, 2))) * (180.0 / 3.14);
  // Mostrar los ángulos separados por un [tab]
  Serial.print("Inclinación en X: ");
  Serial.print(angleX); 
  Serial.print("\tInclinación en Y: ");
  Serial.println(angleY);
  delay(10);

  AjusteX(angleX);
  AjusteY(angleY);
}

void AjusteX(float angleX) {
  if (angleX > 0.5) {
    extendPistonsX(M3IN1, M3IN2, M4IN3, M4IN4, angleX);
  } else if (angleX < -0.5) {
    extendPistonsX(M1IN1, M1IN2, M2IN3, M2IN4, angleX);
  }
}

void AjusteY(float angleY) {
  if (angleY > 0.5) {
    extendPistonsY(M1IN1, M1IN2, M3IN1, M3IN2, angleY);
  } else if (angleY < -0.5) {
    extendPistonsY(M2IN3, M2IN4, M4IN3, M4IN4, angleY);
  }
}

void extendPistonsX(byte IN1, byte IN2, byte IN3, byte IN4, float angle) {
  do {
    // Actualizar lecturas del sensor
    sensor.getAcceleration(&ax, &ay, &az);
    angle = atan(ax / sqrt(pow(ay, 2) + pow(az, 2))) * (180.0 / 3.14);

    // Mover pistones
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
    digitalWrite(IN3, HIGH);
    digitalWrite(IN4, LOW);

    delay(100); // Pequeño retraso para permitir el movimiento
  } while (angle < -0.5 || angle > 0.5);

  // Parar los pistones
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}

void extendPistonsY(byte IN1, byte IN2, byte IN3, byte IN4, float angle) {
  do {
    // Actualizar lecturas del sensor
    sensor.getAcceleration(&ax, &ay, &az);
    angle = atan(ay / sqrt(pow(ax, 2) + pow(az, 2))) * (180.0 / 3.14);

    // Mover pistones
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
    digitalWrite(IN3, HIGH);
    digitalWrite(IN4, LOW);

    delay(100); // Pequeño retraso para permitir el movimiento
  } while (angle < -0.5 || angle > 0.5);

  // Parar los pistones
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}

bestbit · July 1, 2024, 4:26pm

¿Podrías hacer un pequeño esquema de dónde están situados los pistones? Quiero decir, ¿están en las esquinas y, p.e., el 1 está opuesto al 3, o...? Es que veo que siempre activas en el mismo sentido dos pistones y no lo veo muy claro...

i_ricson · July 1, 2024, 6:36pm

Claro, los pistones los tengo acomodados de la siguiente manera.
MESA

Van anclados al piso y al marco de la mesa

bestbit · July 2, 2024, 4:20pm

Buenas tardes. ¿A qué te refieres cuando indicas “pero siento que no es tan eficiente”? Como no sé exactamente a qué haces referencia, y aunque no soy experto ni mucho menos en esto, te hago los siguientes comentarios:

.- Los pistones que utilizas, ¿elevan tan lento como para que no “se pasen” del nivel necesario? (claro que también depende de las dimensiones de la mesa) Veo que los activas dentro del bucle. ¿qué ángulo has calculado que elevan en cada pasada del bucle?. Están activos durante todo el bucle, que incluye una lectura y cálculo de ángulos. Si puedes, indica quáles son.

.- Tal como está el software, veo que en el bucle lees el ángulo, activas los pistones y después verificas si el valor (leido antes de activar los pistones) ya es el correcto. Imagina que hay una inclinación de 0.7º. Entras en el bucle, lees 0.7º y activas pistones. Haces delay de 100ms y comparas. Vuelve al inicio. Lee nuevo angulo y el valor es ya 0.1º. Continuas con los pistones activos, haces delay y comparas. Entonces sales de bucle, pero la última pasada por el bucle ha sido ya cuando la inclinación era menor del margen que quieres. Imagina que por cualcuier razón en esta última pasada se han elevado los pistones y la inclinación ha pasado a -0.6. Como en la verificación indicas mantenerte en el bucle si ángulo<-0.5 o ángulo>0.5, no saldrá nunca del bucle e irá elevando ese lado de la mesa hasta el tope de los pistones.

Cierto que no sé el tipo de pistones que utilizas y si son muy precisos en su movimiento, pero estás suponiendo que realmente se desplazan por igual. Imagina que has estabilizado eje X y ahora vas a por el eje Y. Haces subir los dos hasta dejar Y dentro del márgen válido. Pero uno de ellos va un poco más deprisa: eso provocará que (quizás) de nuevo el ángulo X ya no sea el válido.

i_ricson · July 2, 2024, 8:30pm

Gracias por la respuesta, lo de que no sea tan eficiente es porque activo siempre dos pistones a la vez y tal vez en algun punto se requiera solamente mover 1 o asi.

Los pistones tienen una velocidad de desplazamiento de 3mm/seg y tienen una separacion de 150cm del piston 1 al piston 3, y de 92.5cm del piston 1 al piston 2. Y no tengo ningun tipo de retroalimentacion.

Si, justo eso es lo que me paso en las pruebas, nunca dejaban de subir los pistones y al parecer si suben a diferentes velocidades aunque sean el mismo modelo todos, supongo que son defectos de fabrica.

Surbyte · July 4, 2024, 12:28am

A ver que tal ve este código?

#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;

const int P1_IN1 = 2;
const int P1_IN2 = 3;
const int P1_EN = 4;

const int P2_IN1 = 5;
const int P2_IN2 = 6;
const int P2_EN = 7;

const int P3_IN1 = 8;
const int P3_IN2 = 9;
const int P3_EN = 10;

const int P4_IN1 = 11;
const int P4_IN2 = 12;
const int P4_EN = 13;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  mpu.initialize();

  pinMode(P1_IN1, OUTPUT);
  pinMode(P1_IN2, OUTPUT);
  pinMode(P1_EN, OUTPUT);

  pinMode(P2_IN1, OUTPUT);
  pinMode(P2_IN2, OUTPUT);
  pinMode(P2_EN, OUTPUT);

  pinMode(P3_IN1, OUTPUT);
  pinMode(P3_IN2, OUTPUT);
  pinMode(P3_EN, OUTPUT);

  pinMode(P4_IN1, OUTPUT);
  pinMode(P4_IN2, OUTPUT);
  pinMode(P4_EN, OUTPUT);
}

void loop() {
  int16_t ax, ay, az;
  int16_t gx, gy, gz;
  
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  
  float angleX = atan2(ay, az) * 180/M_PI;
  float angleY = atan2(ax, az) * 180/M_PI;

  Serial.print("AnguloX: ");
  Serial.print(angleX);
  Serial.print(" AnguloY: ");
  Serial.println(angleY);

  // Control logic
  if (angleX > 1) {
    moveMotor(P1_IN1, P1_IN2, P1_EN, HIGH);
    moveMotor(P4_IN1, P4_IN2, P4_EN, LOW);
  } else if (angleX < -1) {
    moveMotor(P1_IN1, P1_IN2, P1_EN, LOW);
    moveMotor(P4_IN1, P4_IN2, P4_EN, HIGH);
  } else {
    stopMotor(P1_EN);
    stopMotor(P4_EN);
  }

  if (angleY > 1) {
    moveMotor(P2_IN1, P2_IN2, P2_EN, HIGH);
    moveMotor(P3_IN1, P3_IN2, P3_EN, LOW);
  } else if (angleY < -1) {
    moveMotor(P2_IN1, P2_IN2, P2_EN, LOW);
    moveMotor(P3_IN1, P3_IN2, P3_EN, HIGH);
  } else {
    stopMotor(P2_EN);
    stopMotor(P3_EN);
  }

  delay(100);
}

void moveMotor(int IN1, int IN2, int EN, bool direction) {
  if (direction) {
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
  } else {
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
  }
  analogWrite(EN, 255); // max velocidad
}

void stopMotor(int EN) {
  analogWrite(EN, 0);
}

Prueba con 1grado y si ves que va bien hazlo con 0.5
Perdón por no seguir tu pinout.
Hay margen para controlar velocidad y que exista una rampa.
Eliminar el delay y hacerlo mas contínuo.

mancera1979 · July 4, 2024, 3:07am

Este proyecto es relativamente similar en cuanto al objetivo.

Entre otras cosas, me llama la atención que reduce a 3 los grados de libertad. Y el enfoque PID es más “eficiente” ya que reduce las oscilaciones y el “overshoot”

i_ricson · July 4, 2024, 3:22pm

Gracias por el aporte, pero ya tengo la estructura y los pistones montados, necesito el control de los 4 pistones. De todas formas me sirve el video ya que planeo fijar uno de los pistones y hacer el balance con los otros 3.

Surbyte · July 4, 2024, 4:16pm

Esto salió de mi charla con ChatGPT (lo odiaba pero ultimamente me dispuse a interactuar).

#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <PID_v1.h>

Adafruit_MPU6050 mpu;

double setpointX = 0, setpointY = 0; // Desired setpoints for leveling (0 degrees for level)
double inputX, inputY;               // Current angles from MPU6050
double outputX, outputY;             // PID outputs

// PID constants
double kp = 4E-4, ki = 2E-6, kd = 7E-3;

PID pidX(&inputX, &outputX, &setpointX, kp, ki, kd, DIRECT);
PID pidY(&inputY, &outputY, &setpointY, kp, ki, kd, DIRECT);

// L298N pins
const int ENA1 = 3, IN1_1 = 4, IN2_1 = 5; // Actuator 1
const int ENA2 = 6, IN1_2 = 7, IN2_2 = 8; // Actuator 2
const int ENA3 = 9, IN1_3 = 10, IN2_3 = 11; // Actuator 3
const int ENA4 = 12, IN1_4 = 13, IN2_4 = 14; // Actuator 4

void setup() {
  Serial.begin(115200);
  
  // Initialize MPU6050
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) { delay(10); }
  }
  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  
  // Initialize PID controllers
  pidX.SetMode(AUTOMATIC);
  pidY.SetMode(AUTOMATIC);
  
  // Initialize L298N pins
  pinMode(ENA1, OUTPUT);
  pinMode(IN1_1, OUTPUT);
  pinMode(IN2_1, OUTPUT);
  
  pinMode(ENA2, OUTPUT);
  pinMode(IN1_2, OUTPUT);
  pinMode(IN2_2, OUTPUT);
  
  pinMode(ENA3, OUTPUT);
  pinMode(IN1_3, OUTPUT);
  pinMode(IN2_3, OUTPUT);
  
  pinMode(ENA4, OUTPUT);
  pinMode(IN1_4, OUTPUT);
  pinMode(IN2_4, OUTPUT);
}

void loop() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  
  // Calculate angles from accelerometer data
  inputX = atan2(a.acceleration.y, a.acceleration.z) * 180 / PI;
  inputY = atan2(a.acceleration.x, a.acceleration.z) * 180 / PI;
  
  // Compute PID outputs
  pidX.Compute();
  pidY.Compute();
  
  // Use PID outputs to control the pistons via L298N
  controlPistons(outputX, outputY);
  
  delay(20); // Wait for 20ms for next reading
}

void controlPistons(double outputX, double outputY) {
  // Map PID outputs to actuator control signals
  int speed1 = map(outputX, -100, 100, -255, 255);
  int speed2 = map(outputY, -100, 100, -255, 255);
  int speed3 = map(outputX, -100, 100, -255, 255);
  int speed4 = map(outputY, -100, 100, -255, 255);
  
  // Control Actuator 1
  analogWrite(ENA1, abs(speed1));
  if (speed1 > 0) {
    digitalWrite(IN1_1, HIGH);
    digitalWrite(IN2_1, LOW);
  } else {
    digitalWrite(IN1_1, LOW);
    digitalWrite(IN2_1, HIGH);
  }
  
  // Control Actuator 2
  analogWrite(ENA2, abs(speed2));
  if (speed2 > 0) {
    digitalWrite(IN1_2, HIGH);
    digitalWrite(IN2_2, LOW);
  } else {
    digitalWrite(IN1_2, LOW);
    digitalWrite(IN2_2, HIGH);
  }
  
  // Control Actuator 3
  analogWrite(ENA3, abs(speed3));
  if (speed3 > 0) {
    digitalWrite(IN1_3, HIGH);
    digitalWrite(IN2_3, LOW);
  } else {
    digitalWrite(IN1_3, LOW);
    digitalWrite(IN2_3, HIGH);
  }
  
  // Control Actuator 4
  analogWrite(ENA4, abs(speed4));
  if (speed4 > 0) {
    digitalWrite(IN1_4, HIGH);
    digitalWrite(IN2_4, LOW);
  } else {
    digitalWrite(IN1_4, LOW);
    digitalWrite(IN2_4, HIGH);
  }
}

revisa conexiones.
Se suponía que no usara los 4 pero a veces no entiende bien. Igual creo que esta interesante.
No se que tal están los valores del PID. Kp Ki y Kd.

i_ricson · July 4, 2024, 5:14pm

Muchas gracias, no confío mucho en los códigos de chatgpt pero sirven como punto de referencia, ya he sacado varios de ahí pero hasta ahora ninguno me ha servido

Surbyte · July 4, 2024, 5:42pm

Si yo mas o menos... los termino ajustando por mi cuenta.
No lo suelo usar con Arduino. Le presto un poco mas de atención en proyectos Backend y Frontend.
Me hace el trabajo grueso en todos.

i_ricson · July 8, 2024, 2:11am

Que tal, modifique el codigo e hice que ajustara piston por piston, comienza por fijar uno, despues ajusta el angulo de las X y por ultimo el de las Y, me falta el ajuste del ultimo piston pero no se como podria implementarlo. Tambien quisiera saber que estructura podria utilizar para detener el piston en cada ajuste haciendo la lectura del MPU6050 cuando el angulo ya sea X o Y este en 0°

#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"

MPU6050 sensor;

// Definiciones de pines
const byte M1IN1 = 2;
const byte M1IN2 = 3;
const byte M2IN3 = 4;
const byte M2IN4 = 5;

const byte M3IN1 = 6;
const byte M3IN2 = 7;
const byte M4IN3 = 8;
const byte M4IN4 = 9;

const byte botonAjuste = 12;
const byte botonReset = 13;

float offsetAx = -560;
float offsetAy = 34;
float offsetAz = 0;

// Valores RAW (sin procesar) del acelerómetro en los ejes x, y, z
int ax, ay, az;

void setup() {
  Serial.begin(57600);    // Iniciando puerto serial
  Wire.begin();           // Iniciando I2C  
  sensor.initialize();    // Iniciando el sensor

  if (sensor.testConnection()) Serial.println("Sensor iniciado correctamente");
  else Serial.println("Error al iniciar el sensor");

  // Configuración de pines de salida
  pinMode(M1IN1, OUTPUT);
  pinMode(M1IN2, OUTPUT);
  pinMode(M2IN3, OUTPUT);
  pinMode(M2IN4, OUTPUT);
  pinMode(M3IN1, OUTPUT);
  pinMode(M3IN2, OUTPUT);
  pinMode(M4IN3, OUTPUT);
  pinMode(M4IN4, OUTPUT);
}

void loop() {
  int btnAj = digitalRead(botonAjuste); 

  if (btnAj = HIGH) {
  int numLecturas = 10;
  float sumaAx = 0, sumaAy = 0, sumaAz = 0;

  sumaAx = 0;
  sumaAy = 0;
  sumaAx = 0;
  
  for (int i = 0; i < numLecturas; i++) {
    sensor.getAcceleration(&ax, &ay, &az);
    sumaAx += ax;
    sumaAy += ay;
    sumaAz += az;
    delay(500);  // Espera un poco entre lecturas
  }
    float promedioAx = sumaAx / numLecturas;
    float promedioAy = sumaAy / numLecturas;
    float promedioAz = sumaAz / numLecturas;

    promedioAx += offsetAx;
    promedioAy += offsetAy;
    promedioAz += offsetAz;

    float AnX = atan(promedioAx/sqrt(pow(promedioAy,2) + pow(promedioAz,2)))*(180.0/3.14);
    float AnY = atan(promedioAy/sqrt(pow(promedioAx,2) + pow(promedioAz,2)))*(180.0/3.14);

    Ajuste(AnX, AnY);
  }

}

void Ajuste(float AnX, float AnY) {
  if (AnX > 0 && AnY > 0) {
    AjustarX(M4IN3, M4IN4);
    delay(1000);
    AjustarY(M1IN1, M1IN2);
    delay(1000);
  } else if (AnX < 0 && AnY > 0) {
    AjustarX(M2IN3, M2IN4);
    delay(1000);
    AjustarY(M3IN1, M3IN2);
    delay(1000);
  } else if (AnX > 0 && AnY < 0) {
    AjustarX(M3IN1, M3IN2);
    delay(1000);
    AjustarY(M2IN3, M2IN4);
    delay(1000);
  } else if (AnX < 0 && AnY < 0) {
    AjustarX(M1IN1, M1IN2);
    delay(1000);
    AjustarY(M4IN3, M4IN4);
    delay(1000);
  }
}

void AjustarX(byte IN1, byte IN2) {
  
        // Actualizar lecturas del sensor
    sensor.getAcceleration(&ax, &ay, &az);
    ax += offsetAx;
    float angleX = atan(ax / sqrt(pow(ay, 2) + pow(az, 2))) * (180.0 / 3.14);

    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
    delay(100);

  // Aqui se debe detener el piston una vez alcanzado el angulo 0 en X
   
    
  }

void AjustarY(byte IN1, byte IN2) {
  
        // Actualizar lecturas del sensor
    sensor.getAcceleration(&ax, &ay, &az);
    ay += offsetAy;
    float angleY = atan(ay / sqrt(pow(ax, 2) + pow(az, 2))) * (180.0 / 3.14);

    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
    delay(100);

 // Aqui se debe detener el piston una vez alcanzado el angulo 0 en Y
   
    
  }

Espero y me puedan apoyar con sugerencias

Topic		Replies	Views
Problema con MPU5060 Giroscopio Software	1	930	May 7, 2021
sensor mpu6050 Software	2	683	May 7, 2021
Código MPU6050 + sensores i2c + serial Software	8	3073	May 7, 2021
Medidor de Inclinación Hardware	3	599	May 7, 2021
con este codigo de robot Software	2	627	May 7, 2021

Mesa auto-nivelable

Related Topics