Self Balancing Robot with Yellow Motors on Smooth Surface

Posted on 04/06/2024 by demej00
Modified on: 12/06/2024
Steps completed / 2
Press to mark a step as
completed or click here to complete all
Components you will need
Select missing items to add them
to the cart or select all
Introduction

Demo and explanation 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.

I built this robot 6 years ago and it balanced then on a smooth surface but later could not replicate it - now I know why and how. It is all about timing, frequency - Tesla was right.


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;

}

 

```

Flag this post

Thanks for helping to keep our community civil!


Notify staff privately
It's Spam
This post is an advertisement, or vandalism. It is not useful or relevant to the current topic.

You flagged this as spam. Undo flag.Flag Post