diff --git a/README.md b/README.md index 5f64807f0708784871316a586bec58d6480c2460..a2175ccf3e7b91b7501b6e4ba2a941f630e88b9d 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,103 @@ # proyecto_instrumentacion +## Codigo de arduino usado para el control del motor +``` + +#include "I2Cdev.h" + +#include "MPU6050.h" + +#include "Wire.h" + +#include <AccelStepper.h> + +#define DIR_PIN 9 + +#define STEP_PIN 8 + +AccelStepper stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN); + +MPU6050 sensor; + +int ax, ay, az; + int target_angle = 0; + int tolerance = 1; +const int valid_angles[] = {-220,-200,-180,-165,-150,-135,-120,-105,-90,-75,-60,-45,-30,-20, -15, -10, -5,0,15,30,45,60,75, 90,105,120,135,150,165,180,200, 220}; + +void setup() { + Serial.begin(57600); + Wire.begin(); + sensor.initialize(); + + stepper.setMaxSpeed(1000); + stepper.setAcceleration(500); + + if (sensor.testConnection()) + Serial.println("Sensor iniciado correctamente"); + else + Serial.println("Error al iniciar el sensor"); +} + +void loop() { + if (Serial.available()) { + int input_angle = Serial.parseInt(); +if (isValidAngle(input_angle)) { + target_angle = input_angle; + Serial.print("Moviendo a "); + Serial.print(target_angle); + Serial.println(" grados..."); + rotateToTargetAngle(); +} else { + Serial.println("Ãngulo no válido. Ingrese un múltiplo de 15 grados."); +} + +while (Serial.available()) + Serial.read(); // Limpiar el búfer del monitor serial + + } + + sensor.getAcceleration(&ax, &ay, &az); + int current_angle = calculateAngle(); + + if (fabs(current_angle - target_angle) <= tolerance) { + stepper.stop(); + Serial.println("Motor detenido."); + } +} + +int calculateAngle() { + int accel_ang_x = atan(ax / sqrt(pow(ay, 2) + pow(az, 2))) * (180.0 / 3.14); + int accel_ang_y = atan(ay / sqrt(pow(ax, 2) + pow(az, 2))) * (180.0 / 3.14); + return accel_ang_x; // Puedes cambiar esto a accel_ang_y si prefieres el ángulo en Y +} + +void rotateToTargetAngle() { + float steps = target_angle / 0.9; // 1.8 grados por paso del motor + + if (steps > stepper.currentPosition()) { + stepper.setSpeed(1000); + stepper.moveTo(steps); + } else { + stepper.setSpeed(-1000); + stepper.moveTo(steps); + } + + while (stepper.distanceToGo() != 0) { + stepper.run(); + } +} + +bool isValidAngle(int angle) { + for (int i = 0; i < sizeof(valid_angles) / sizeof(valid_angles[0]); i++) { + if (angle == valid_angles[i]) { + return true; + } + } + return false; +} +``` ## Getting started To make it easy for you to get started with GitLab, here's a list of recommended next steps.