Self Balancing Robot with Yellow Motors and No Encoders on Smooth Surface

Demo and explaination of how to get a yellow motor Self Balancing Robot with no encoders to balance on a smooth surface. Mostly my research and experiences of finding that good balance is achieved with a correct timing loop and P and I terms of the PID controller.
Hardware: Arduino Uno, L298N motor driver, MPU6050, 48:1 150rpm yellow geared motors (no encoders), 850mah lipo battery to power L298N and motors, and 9v rechargable battery to power Arduino Uno.

[@haipingcao2212_.]

Kp

[@demej00]

Here is complete Arduino code with huge comment section to explain setup and details. Hope you make one!
// jim demello jun 2024 - short Arduino code for SBR (self balancing robot) using 48:1, 150 rpm yellow motors with no encoders
// majority of code taken from midhun_s www.instructables.com/Arduino-Self-Balancing-Robot-1/
//
// hardware: 1.a. L298N motor driver - be sure jumpers are on enable pins or will not work. These drivers are cheap, I had a bad one
// so buy more than one.
// 1.b. Arduino Uno - also had a bad one where one wheel would only turn in one direction. Replaced Uno and it was good.
// 2. I used two potentiometers to tune targetAngle and Proportional term but you don't have to, but it makes life easier to do so
// 3. MPU6050 - be sure you calibrate it, don't use someone elses values, like mine.
// MPU6050 - be sure you mount it with the X axis parallel to the axis of the wheels.
// 4. power - I used 2s lipo to power L298N. So a fully charged 2s lipo, 8.4 volts will deliver about 6.4 to 7 volts to motors.
// And I used rechargeable 9 volt battery to power the Arduino Uno when not connected to USB. Lasts about 15 minutes.
// 5. motors - 48:1 geared yellow motors, 150 rpm at 6 volts. No encoders required. Encoders will be necessary to
// control tracking and fast robot response. Will do that someday.
// 6. wheels - larger and heavier wheels make for better control. 65mm are ok but 80mm are great.
// 7 counterweight - mount lipo on top of SBR for better control - SBR falls over slower with weight on top -
// larger wheels and counterweight can compensate for slower motors.
//
// Notes: 1. be sure wheels are turning towards direction of lean.
// 2. do not use low baud rate for Serial which will result in a timing loop too slow to work.
// 3. according to this article: polyengineer.wordpress.com/2014/08/08/self-balancing-robot-pid-control/
// you should use 5 times Proportional term for Integral term value. It works. I use up to 7 times.
// This prevents drifting (except on uneven surface - need encoders to stop that). Not using Derivative so this is a PI controller.
// 4. targetAngle is another term for setPoint.
// 5. don't use delay() - it will mess up the timing.
//
// My opinion: 1. an inverted pendulum like an SBR has a natural frequency that needs to be matched by the timing of the loop
// and the Proportional term. If P is too high then it becomes a bang-bang machine, on and off - effective
// but ugly. The timing loop must be consistent for good balancing or it will jump all over the place.
// So print out how long the loop takes to determin its Hz. A Hz of 50 to 60 is ideal - about 20 milliseconds.
// Be sure all of your code is inside the timing loop or strange things can happen.
// Timing loop can run from 30 down to 5 milliseconds.
// 2. The high Integral value (5 times P) prevents drifting.
// 3. This is the shortest, simplest and most effective code to self balance a two wheeled robot - I think.
// 4. This robot will balance and stay in place pretty much on a smooth surface like a stone floor. Most robots
// with these motors will not balance on a stone floor and/or they will drift off and fall over.
// They will balance only on carpet or rough surfaces. I think it is due to bad timing loop.
// 5. MPU6050 Complementary filter code here is effective, maybe more effective than MPU6050 DMP values which seem to jump
// around more than these. Haven't tried Kalman filter - looks too complicated for me.
// 6. The motors can be optomized by matching the point at which they both begin to move at the same time by
// adding or subtracting from one of the motors motorSpeed values. Haven't done that here as I wanted the code
// stripped down to it's essentials.
// 5. A great thanks to midhun_s.
// 6. I started work on this robot 6 years ago and finally got it working to my satisfaction this year.
// I accidently had it working 6 years ago on a smooth surface but never knew why. Now I think I know. Never give up.
// 7. Opinions are overrated by their holders.
//
#include "MPU6050.h"
// vars for mpu6050 //
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
#define leftMotorPWMPin 6
#define leftMotorDirPin 7
#define rightMotorPWMPin 5
#define rightMotorDirPin 4
float sampleTime ;
MPU6050 mpu;
int16_t accY, accZ, gyroX;
volatile int motorPower, gyroRate;
volatile float accAngle, gyroAngle, currentAngle, prevAngle=0, error, prevError=0, errorSum=0;
//////////////////////////////////////////////////////////
volatile float Kp=25,Ki=0,Kd=0,targetAngle=0;
//////////////////////////////////////////////////////////
float adjustAngle = 0;
unsigned long currentMillis;
long previousMillis = 0;
long loopTimer;
#define PRINTLN(...) Serial.println(__VA_ARGS__)
void setMotors(int leftMotorSpeed, int rightMotorSpeed) {
if(leftMotorSpeed >= 0) {
analogWrite(leftMotorPWMPin, leftMotorSpeed);
digitalWrite(leftMotorDirPin, LOW);
}
else { // if leftMotorSpeed is < 0 then set dir to reverse
analogWrite(leftMotorPWMPin, 255 + leftMotorSpeed);
digitalWrite(leftMotorDirPin, HIGH);
}
if(rightMotorSpeed >= 0) {
analogWrite(rightMotorPWMPin, rightMotorSpeed);
digitalWrite(rightMotorDirPin, LOW);
}
else {
analogWrite(rightMotorPWMPin, 255 + rightMotorSpeed);
digitalWrite(rightMotorDirPin, HIGH);
}
}

void setup() {
Serial.begin(115200); // do not go low as in 9600 or timing loop will be very slow
// set the motor control and PWM pins to output mode
pinMode(leftMotorPWMPin, OUTPUT);
pinMode(leftMotorDirPin, OUTPUT);
pinMode(rightMotorPWMPin, OUTPUT);
pinMode(rightMotorDirPin, OUTPUT);

// initialize the MPU6050 and set offset values
Serial.print("Initialize MPU at: ");Serial.println(millis());
mpu.initialize();
int con = mpu.testConnection(); // test MPU6050 - I had some problems so this was necessary for me
PRINTLN(con ? F("connection successful") : F("connection failed"));
mpu.setYAccelOffset(2067); // from calibration routine - use your own calibration values
mpu.setZAccelOffset(1207);
mpu.setXGyroOffset(4);
Serial.print("End Initialize MPU at: ");Serial.println(millis());
delay(1000);

loopTimer = 15;
sampleTime =0.015;
} // end setup()
void loop() {
int noMillis;
currentMillis = millis();
noMillis = currentMillis - previousMillis;
// main timing loop
if(noMillis > loopTimer) {
targetAngle= float(map(analogRead(A0), 0, 1023, -600,600))/100;
//Kp = map(analogRead(A1), 0, 1023, 0,800);

Kp = 60;
Kd = .00;
Ki = 500;

// read acceleration and gyroscope values
accY = mpu.getAccelerationY();
accZ = mpu.getAccelerationZ();
gyroX = mpu.getRotationX();
previousMillis = currentMillis;
doPID();

motorPower = constrain(motorPower, -255, 255);

if (abs(error) > 15) motorPower = 0; // if fall over then shut off motors

Serial.print("Kp= ");Serial.print(Kp);
Serial.print(" currentAngle= ");Serial.print(currentAngle);
Serial.print(" error= ");Serial.print(error);
Serial.print(" errorSum= ");Serial.print(errorSum);
Serial.print(" adjustAngle= ");Serial.print(adjustAngle);
Serial.print(" targetAngle= ");Serial.print(targetAngle);
Serial.print(" noMillis= ");Serial.print(noMillis);
Serial.print(" motorPower= ");Serial.println(motorPower);

setMotors(motorPower , motorPower );
} // end timer
} // end main loop
void doPID()
{
// calculate the angle of inclination
accAngle = atan2(accY, accZ)*RAD_TO_DEG;
gyroRate = map(gyroX, -32768, 32767, -250, 250);
gyroAngle = (float)gyroRate*sampleTime;
// complementary filter ///////////////////////////////////////////
currentAngle = 0.9934*(prevAngle + gyroAngle) + 0.0066*(accAngle);
//////////////////////////////////////////////////////////////////
error = currentAngle - targetAngle + adjustAngle + 0;
errorSum = errorSum + error;
errorSum = constrain(errorSum, -250, 250); // clamp Integral

//calculate output from P, I and D values ///////////////////////////////////////////////////
motorPower = Kp*(error) + Ki*(errorSum)*sampleTime - Kd*(currentAngle-prevAngle)/sampleTime;
/////////////////////////////////////////////////////////////////////////////////////////////

prevAngle = currentAngle;
}

[@haipingcao2212_.]

Empty