#include <AccelStepper.h>
#include <Wire.h>
#include <MPU6050.h>
#include <math.h>
#define dirPin 2
#define stepPin 3
#define buttonCW 4 // Button for clockwise rotation
#define buttonCCW 5 // Button for counterclockwise rotation
#define buttonSweep 6 // Button for 180-degree continuous sweep
#define buttonMPU 7 // Button for MPU6050 control
#define enPin 10
AccelStepper stepper(AccelStepper::DRIVER, stepPin, dirPin);
MPU6050 mpu;
int mpuControl = 0;
int manualControl = 0;
bool sweeping = false;
int sweepSteps = 200; // Steps for 180-degree rotation, adjust for your stepper motor
void disableStepper() {
digitalWrite(enPin, HIGH); // Disable stepper en pin
}
void enableStepper() {
digitalWrite(enPin, LOW); // Enable stepper en pin
}
void setup() {
Serial.begin(9600);
pinMode(enPin, OUTPUT); // Set enable pin as low
pinMode(buttonCW, INPUT_PULLUP);
pinMode(buttonCCW, INPUT_PULLUP);
pinMode(buttonSweep, INPUT_PULLUP);
pinMode(buttonMPU, INPUT_PULLUP);
stepper.setMaxSpeed(1000);
stepper.setAcceleration(500);
Wire.begin();
mpu.initialize();
}
void loop() {
if (digitalRead(buttonCW) == LOW) {
manualControl = 1;
sweeping = false; // Stop sweeping if another button is pressed
rotateClockwise();
} else if (digitalRead(buttonCCW) == LOW) {
manualControl = 1;
sweeping = false; // Stop sweeping if another button is pressed
rotateCounterclockwise();
} else if (digitalRead(buttonSweep) == LOW) {
manualControl = 1;
mpuControl = 0; // Turn off MPU control if the sweep button is pressed
sweeping = true;
enableStepper(); // Enable stepper motor
} else if (digitalRead(buttonMPU) == LOW) {
enableStepper(); // Enable stepper motor
mpuControl = 1;
manualControl = 0;
sweeping = false; // Stop sweeping when MPU control is enabled
}
if (sweeping) {
continuousSweep(); // Call the sweep function repeatedly
}
if (mpuControl) {
controlWithMPU();
}
}
void rotateClockwise() {
enableStepper(); // Enable Stepper Motor
while (digitalRead(buttonCW) == LOW) {
stepper.move(1); // Rotate 1 step
stepper.run();
if (anyOtherButtonPressed()) {
disableStepper(); // Disable Stepper Motor
break;
}
}
disableStepper(); // Disable Stepper Motor
}
void rotateCounterclockwise() {
enableStepper(); // Enable Stepper Motor
while (digitalRead(buttonCCW) == LOW) {
stepper.move(-1); // Rotate 1 step in the opposite direction
stepper.run();
if (anyOtherButtonPressed()) {
disableStepper(); // Disable Stepper Motor
break;
}
}
disableStepper(); // Disable Stepper Motor
}
void continuousSweep() {
static int direction = 1; // 1 for forward, -1 for backward
if (!sweeping) return; // Only sweep if the sweep button is pressed
// Move 180 degrees in the current direction
stepper.move(direction * sweepSteps);
while (stepper.distanceToGo() != 0) {
stepper.run();
// Check if any other button is pressed to stop sweeping
if (anyOtherButtonPressed()) {
stepper.stop();
sweeping = false;
disableStepper();
return;
}
}
delay(500); // Small delay at the end of each sweep
// Reverse direction for the next sweep
direction = -direction;
}
bool anyOtherButtonPressed() {
return (digitalRead(buttonCW) == LOW || digitalRead(buttonCCW) == LOW || digitalRead(buttonMPU) == LOW);
}
void controlWithMPU() {
int16_t ax, ay, az, gx, gy, gz;
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
// Calculate pitch angle in degrees (simplified calculation using accelerometer data)
float pitch = atan2(ax, sqrt(ay * ay + az * az)) * 180 / PI;
// Rotate the motor based on pitch angle
if (pitch > 5) { // Threshold to avoid minor vibrations
stepper.move(1); // Rotate clockwise for positive pitch
} else if (pitch < -5) {
stepper.move(-1); // Rotate counterclockwise for negative pitch
}
stepper.run();
// Stop MPU control if any button is pressed
if (anyOtherButtonPressed()) {
disableStepper();
mpuControl = 0;
}
}