Self Balancing Robot with Yellow Motors on Smooth Surface

This video is a blurb about how to get self balancing robots using cheap yellow motors to balance well enough to balance even on a smooth surface. Most of these robots that do not use encoders wander around and fall over unless they are balancing on a rough surface. It seems that the timing loop is critical to good balancing and a high Integral term for the PID controller is necessary to prevent drifting. Note, these robots without encoders will drift on an uneven surface.

These yellow motors are typically 48:1 ratio, 150 rpm at 6 volts. They have a terrible amount of backlash and it is surprising they can function at all, but they do.

 

After making the above version, I added the RoboRemo Bluetooth app to control the PID parms from my phone and show that just by adding 5 times P parm for Integral parm stops the robot from drifting away on a smooth surface. Shown in video below.

 

 

```

// 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 https://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: https://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 byte count=0;

//////////////////////////////////////////////////////////

volatile float Kp=25,Ki=0,Kd=0,targetAngle=0;

//////////////////////////////////////////////////////////

 

float adjustAngle = 0;

unsigned long currentMillis;

long previousMillis = 0;  

long loopTimer = 5;

 

#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 = .0001;

       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 );

     //setMotors(60 , 65 );

   } // 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;

}

 

```