Hey everyone, ive been developing my own self balancing robot for a while now, ive managed to get as far as a working program and pcb, everything is working except for one part which has been annoying me because its throwing everything off. Im using an mpu6050 as the gyroscope angle reading, two nema17 stepper motors with a4988 drivers, and an esp32. when the motors arent being powered the program works fine and everything is accurate, but as soon as i power the motors, the angle reading begins to fluctuate erratically and inaccurately and jumps a degree or two randomly.
I have spent like a month trying to figure out what is causing it, initially i thought EMI or a grounding issue, but I tested these theories out and Im pretty confident they're not the issue. I've put capacitors on every spot that could be causing electrical noise, ive tried replacing every part, used separate power supplies, etc.
Im thinking the issue might lie in the code, so if anyone can notice an obvious flaw in the code which is causing this issue or something that can fix it, I would appreciate a ton. A lot of the code has been modified with CHATGPT when i was trying to find solutions lol.
video:
#include <Wire.h>
#include <MPU6050_light.h>
#include <PID_v1.h>
// Stepper motor pins
#define DIR_PIN 2
#define STEP_PIN 4
#define DIR_PIN2 12
#define STEP_PIN2 14
// MPU6050 object
MPU6050 mpu(Wire);
// PID variables
double setpoint = 0; // Desired angle of 0 degrees
double input, output;
// PID gain control
double Kp = 550.0;
double Ki = 0.0;
double Kd = 15.0;
// PID controller object
PID myPID(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);
// Timing variables
unsigned long lastPIDUpdate = 0;
const unsigned long PIDInterval = 15;
// Motor control variables
int motorSpeed = 0; // Motor speed controlled by PID
unsigned long lastStepTime1 = 0;
unsigned long lastStepTime2 = 0;
// Task handles
TaskHandle_t pidTaskHandle;
// Function prototypes
void pidLoop(void *parameter);
void motorControl();
void setup() {
Serial.begin(115200);
// Initialize MPU6050
Wire.begin();
byte status = mpu.begin();
if (status != 0) {
Serial.println("MPU6050 connection failed");
while (1); // Stop if connection fails
}
Serial.println("MPU6050 initialized successfully");
// Calibrate MPU6050
mpu.calcOffsets(true, true); // Calibrate gyro and accelerometer
// Initialize motor pins
pinMode(DIR_PIN, OUTPUT);
pinMode(STEP_PIN, OUTPUT);
pinMode(DIR_PIN2, OUTPUT);
pinMode(STEP_PIN2, OUTPUT);
// Initialize PID controller
myPID.SetMode(AUTOMATIC);
myPID.SetOutputLimits(-10000, 10000); // Adjust output limits
// Create a task for the PID loop on core 0
xTaskCreatePinnedToCore(pidLoop, "PID Task", 10000, NULL, 2, &pidTaskHandle, 0); // Core 0, priority 2
}
void loop() {
// Motor control logic runs on core 1
motorControl();
}
void pidLoop(void *parameter) {
while (true) {
unsigned long currentMillis = millis();
// Update MPU6050 and PID at regular intervals
if (currentMillis - lastPIDUpdate >= PIDInterval) {
lastPIDUpdate = currentMillis;
// Update MPU6050 angle
mpu.update();
double angle = mpu.getAngleX(); // Get the X-axis angle
// Lightweight smoothing (reduces noise without adding latency)
static double smoothedAngle = 0.0;
smoothedAngle += 0.3 * (angle - smoothedAngle); // Exponential smoothing
// Update PID controller
input = smoothedAngle;
myPID.Compute();
// Map PID output to motor speeds
motorSpeed = (int)output;
// Set motor directions
digitalWrite(DIR_PIN, motorSpeed > 0);
digitalWrite(DIR_PIN2, motorSpeed > 0);
// Print debug info
Serial.print("Angle: ");
Serial.print(smoothedAngle);
Serial.print(" Motor Speed: ");
Serial.println(motorSpeed);
}
// Use delay to yield time for other tasks
delay(1);
}
}
void motorControl() {
// Generate step pulses for motor 1
if (abs(motorSpeed) > 0) {
unsigned long stepInterval1 = 1000000 / abs(motorSpeed); // Microseconds per step
if (micros() - lastStepTime1 >= stepInterval1) {
lastStepTime1 = micros();
digitalWrite(STEP_PIN, HIGH);
delayMicroseconds(2); // Pulse width
digitalWrite(STEP_PIN, LOW);
}
}
// Generate step pulses for motor 2
if (abs(motorSpeed) > 0) {
unsigned long stepInterval2 = 1000000 / abs(motorSpeed); // Microseconds per step
if (micros() - lastStepTime2 >= stepInterval2) {
lastStepTime2 = micros();
digitalWrite(STEP_PIN2, HIGH);
delayMicroseconds(2); // Pulse width
digitalWrite(STEP_PIN2, LOW);
}
}
}
schematic:
