Hello everyone, I am developing a project which consists of leveling a table automatically by means of 4 electric pistons with DC motors controlled with L298N, plus a MPU6050 sensor to obtain the leveling angles, I would like to know what you think of the following code and what improvements could be included or change.
What the code does is to measure the angles with the MPU and then with trigonometry calculate the necessary displacement for the pistons, which have a displacement speed of 2.6mm/s, and do not have any feedback. then first move two pistons to level the X axis, and then the other two to level the Y axis, but I feel that it is not that efficient.
Here is the code.
#include "I2Cdev.h"
#include <MPU6050.h>
#include <Wire.h>
MPU6050 mpu;
// Pinout
const byte M1IN1 = 2; //motor 1
const byte M1IN2 = 3;
const byte M2IN3 = 4; //motor 2
const byte M2IN4 = 5;
const byte M3IN1 = 6; //motor 3
const byte M3IN2 = 7;
const byte M4IN3 = 8; //motor 4
const byte M4IN4 = 9;
//buttons
const byte buttonAdjust = 12;
// Constants of displacement (in mm)
const float DIST_PISTON1 = 1500; // 150 cm
const float DIST_PISTON2 = 925; // 92.5 cm
int ax, ay, az;
void setup() {
Serial.begin(9600);
Wire.begin();
mpu.initialize();
// Setup pins of motors as outputs
pinMode(M1IN1, OUTPUT);
pinMode(M1IN2, OUTPUT);
pinMode(M2IN3, OUTPUT);
pinMode(M2IN4, OUTPUT);
pinMode(M3IN1, OUTPUT);
pinMode(M3IN2, OUTPUT);
pinMode(M4IN3, OUTPUT);
pinMode(M4IN4, OUTPUT);
// Setup buttons as INPUT
pinMode(buttonAdjust, INPUT);
digitalWrite(M1IN1, LOW);
digitalWrite(M1IN2, LOW);
digitalWrite(M2IN3, LOW);
digitalWrite(M2IN4, LOW);
digitalWrite(M3IN1, LOW);
digitalWrite(M3IN2, LOW);
digitalWrite(M4IN3, LOW);
digitalWrite(M4IN4, LOW);
if (mpu.testConnection()) {
Serial.println("MPU6050 connected correctly");
} else {
Serial.println("Error connecting MPU6050");
}
}
void loop() {
int btnAdj = digitalRead(buttonAdjust);
// Obtaining readings from the MPU6050
mpu.getAcceleration(&ax, &ay, &az);
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);
if (btnAdj == HIGH) {
float radAngleX = radians(angleX); // Desired angle X
float radAngleY = radians(angleY); // Desired angle Y
// Calcular desplazamiento
double displacementX = (tan(radAngleX) * DIST_PISTON1);
double displacementY = (tan(radAngleY) * DIST_PISTON2);
//Calculate displacement time
int TimeX = ((displacementX * 1000)/ 2.6); //The pistons have a displacement speed of 2.6mm/s
int TimeY = ((displacementY * 1000)/ 2.6);
int TdispX = abs(TimeX);
int TdispY = abs(TimeY);
Serial.print("Inclination X: ");
Serial.print(angleX);
Serial.print("Inclination Y:");
Serial.println(angleY);
Serial.println(displacementX);
Serial.println(displacementY);
Serial.println(TdispX);
Serial.println(TdispY);
delay(1000);
Adjust(TdispX, TdispY, angleX, angleY);
}
}
void Adjust(int TdispX, int TdispY, float angleX, float angleY) {
Serial.println("Adjusting X");
float anX = angleX;
if (anX > 0) {
extendPistons(M3IN1, M3IN2, M4IN3, M4IN4, TdispX);
} else if (anX < 0) {
extendPistons(M1IN1, M1IN2, M2IN3, M2IN4, TdispX);
}
Serial.println("Ajust X completed");
AdjustY(TdispY, angleY);
}
void AdjustY(int TdispY, float angleY) {
Serial.println("Adjusting Y");
float anY = angleY;
if (angleY > 0) {
extendPistons(M1IN1, M1IN2, M3IN1, M3IN2, TdispY);
} else if (angleY < 0){
extendPistons(M2IN3, M2IN4, M4IN3, M4IN4, TdispY);
}
Serial.println("Ajust Y completed");
return;
}
void extendPistons(byte IN1, byte IN2, byte IN3, byte IN4, int TIME) {
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
delay(TIME);
digitalWrite(IN1, LOW);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4, LOW);
}
And an image showing how I have the pistons arranged and the dimensions.
