Hello! I am trying to build a self balancing robot and it is working really bad and has some very strange bugs. Firstly here are the components i am using:
- arduino uno
- MPU6050 accelerometer and gyro
- 2 steppers 17HS4401 (1.7 A, 40 N⋅cm)
- 2 stepper drivers A4988
And here is the code:
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "math.h"
#include <AccelStepper.h>
#include <MultiStepper.h>
#include <avr/wdt.h>
#define leftMotorPWMPin 9
#define leftMotorDirPin 8
#define rightMotorPWMPin 11
#define rightMotorDirPin 10
#define Kp 10
#define Kd 0.1
#define Ki 0
#define sampleTime 0.005
#define targetAngle -2.5
MPU6050 mpu;
// Initialize AccelStepper for two motors
AccelStepper stepper1(AccelStepper::DRIVER, leftMotorPWMPin, leftMotorDirPin);
AccelStepper stepper2(AccelStepper::DRIVER, rightMotorPWMPin, rightMotorDirPin);
int16_t accY, accZ, accX, gyroX;
volatile int motorPower, gyroRate;
volatile float accAngle, gyroAngle, currentAngle, prevAngle=0, error, prevError=0, errorSum=0;
volatile byte count=0;
int distanceCm;
void init_PID() {
// Stop the timer
TCCR1B = 0;
// Set the timer to CTC mode
TCCR1A = 0; // Ensure WGM10 and WGM11 are both 0
TCCR1B |= (1 << WGM12); // Set WGM12 to 1
TCCR1B &= ~(1 << WGM13); // Ensure WGM13 is 0
// Set the compare match register to 1249
OCR1A = 1249;
// Set the prescaler to 64 and start the timer
TCCR1B |= (1 << CS11) | (1 << CS10); // CS11 = 1 and CS10 = 1
// Enable Timer Compare Interrupt
TIMSK1 |= (1 << OCIE1A); // Set the OCIE1A bit high to enable the interrupt
// Ensure global interrupts are enabled
sei();
}
void setup() {
// set the motor control and PWM pins to output mode
pinMode(leftMotorPWMPin, OUTPUT);
pinMode(leftMotorDirPin, OUTPUT);
pinMode(rightMotorPWMPin, OUTPUT);
pinMode(rightMotorDirPin, OUTPUT);
Serial.begin(115200);
// initialize the MPU6050 and set offset values
mpu.initialize();
mpu.setXAccelOffset(594);
mpu.setYAccelOffset(3774);
mpu.setZAccelOffset(1398);
mpu.setXGyroOffset(148);
mpu.setYGyroOffset(86);
mpu.setZGyroOffset(6);
stepper1.setMaxSpeed(1000);
stepper1.setAcceleration(500);
stepper2.setMaxSpeed(1000);
stepper2.setAcceleration(500);
wdt_enable(WDTO_2S);
// initialize PID sampling loop
init_PID();
}
volatile bool updateFlag = false; // Flag to signal when to update PID calculations
ISR(TIMER1_COMPA_vect) {
updateFlag = true; // Set the flag to true to indicate data should be processed
}
void loop() {
wdt_reset();
if (updateFlag) {
// Reset the flag
updateFlag = false;
// Perform sensor reading and calculations
readSensors();
calculatePID();
// Update motors
stepper1.setSpeed(-motorPower);
stepper2.setSpeed(motorPower);
// Serial.println(motorPower);
}
stepper1.runSpeed();
stepper2.runSpeed();
}
void readSensors() {
accY = mpu.getAccelerationY();
accZ = mpu.getAccelerationZ();
accX = mpu.getAccelerationX();
gyroX = mpu.getRotationX();
accAngle = atan2(accX, accZ) * RAD_TO_DEG;
gyroRate = map(gyroX, -32768, 32767, -250, 250);
gyroAngle = (float)gyroRate * sampleTime;
}
void calculatePID() {
currentAngle = 0.9934 * (prevAngle + gyroAngle) + 0.0066 * (accAngle);
error = currentAngle - targetAngle;
errorSum += error;
errorSum = constrain(errorSum, -300, 300);
motorPower = Kp * error + Ki * errorSum * sampleTime - Kd * (currentAngle - prevAngle) / sampleTime;
motorPower = constrain(motorPower, -500, 500);
prevAngle = currentAngle;
}
I am sampling from the mpu6050 and modifying the motor power with an interrupt at 5ms on timer1.
A strange bug that happens here is that the program will just stop working at random times. It will not even print anything to the serial. I read that it might be from the timer i am using maybe it conflicts with something else? To temporarily solve that i set up a watchdog that resets the arduino but i want get rid of this bug if i can.
Now for the balancing, The robot actually oscillates well in the balanced position and it can stay like that but at any push that is not a very light touch it will not be able to recover. Do you have any advice on how to tune the pid constants to achieve a better recovery?.
Another thing that is bothering me is that the steppers, while they are working they seem to have a very forced movement like they are struggling to make every step and they are not smooth at all. Any idea why that might happen?
Thank you for your time and I appreciate any ideas since I am really lost!
