Self Balancing Robot Coding Issue

Projects Programming
1

Hi guys,
I'm trying to build a Self Balancing Robot as project for my university course. Right now i can't reach balance and i think the problems are with my code. I'm not interested in moving back and forth, just a decent balance.

I'm trying to give as much information i can, feel free to ask more details if needed.

Materials and Geometry

3x Layers of Plastic Glass (16x10x0.5 cm)
4x Threaded Rod (M6)

Height from axes of rotation --> 20cm
Weight: 950g

Photo: Self Balancing Robot - Album on Imgur

Components:

Arduino Uno
L298N
MPU6050
2x 280rpm Motors
2x 3.7V Li-Ion 18650 Batteries

I have read a lot of topics and did multiple researches on this subject and I think I have a good understand on how in theory every single component works. I have also understand principles of PID and his parameters, at least in theory.

In the first days I tried to implement a code that seems to be very popular online. It uses a library called "LMotorController" (Check Attachments) but didn't work for me.

In this moment I'm trying to write my own way to tell motors how they have to move. Also trying to fix dead zone with map function.

I believe I'm still far away from the final part when you have to "only" adjust kp, ki, kd.

Edit:
I try to explain me better. Right now my bot can't stay on balance, I think the motors are not responding properly to the pid output. As I said i have understood how mpu read the data, how pid works and so on but i can't figure how to properly control motors. In this moment I'm trying to implement map function to avoid dead zones of motors

Thank you for your attention,
Alessio

LMotorController.cpp (3.77 KB)

LMotorController.h (594 Bytes)

OnlineCode.ino (4.37 KB)

SBR_nov24a.ino (4.74 KB)

2

Please read and follow the directions in the "How to use this forum" post. You have not provided the information required for anyone to be able to help.

For example, explain exactly what you mean by "can't reach balance".

What is supposed to happen, and what happens instead?

What steps have you taken to fix the problem, and what was the result?

Do you understand every part of the programs you downloaded? I could be wrong, but for a university course, that should be the least that your instructor expects of you.

3

280rpm seems slow for a balance robot. Are they geared motors?

Only 7.2V to drive the L298N is very low voltage. You lose a lot of that voltage in the driver chip.

4

Hi jremington thank you for point me to the right direction. Just to clarify I'm studying Mechanical Engineering (3rd year) and this project is for an optional course called "Mechanical's Vibration". So i don't have a strong background with programming and elettronics but I'm willing to learn.

@MorganS The motors are DC Motors 6V 280rpm. I thought that was fine because I have seen this design very popular online. Could you provide me some examples of Motors + Batteries that you think are fine?

5

The motors are DC Motors 6V 280rpm.

What is the motor stall current?

In addition to the low battery voltage mentioned above, the L298 motor driver is very inefficient and cannot handle the current required by most motors used these days in robots.

Pololu has a good selection of modern, efficient motor drivers.

6

Operating voltage: between 3 V and 9 V
Nominal voltage: 6 V
Free-run speed at 6 V: 281 RPM
Free-run current at 6 V: 80 mA
Stall current at 6V: 900 mA
Stall torque at 6V: 4 kg·cm
Gear ratio: 1:21
Reductor size: 19 mm
Weight: 84 g

I thought that an improvment in the motor driver and motors could lead me to a better result but I also see a lot of bot that works well with this setup.

I will definitely check Polulu website :slight_smile:

7

The stall current of 900 mA is OK for the L298, but you lose 2 to 4 V as heat.

That is why you need higher battery voltage.

8

I can add another battery and reach 11,1V. However I still have my doubt on my code obviously

9

Please answer the questions in reply #1.

10

I did an edit to my original post in which i tried to answer your questions. However I'm gonna add some details.

For example, explain exactly what you mean by "can't reach balance".

I feel that my motors don't respond properly to my pid output. Therefore my robot is not able to balance itself even for a few seconds. So I suppose i'm not yet in the tuning part where I have to set Kp, Ki and Kd to achieve a perfect balance.

What is supposed to happen, and what happens instead?
In this state I expected to have an unstable equilibrium and adjust it with some pid tuning. Instead motors seems to not react to my output. I leave the bot in stable position, it starts to fall and motors doesn't react. (Later on i will add a video)

What steps have you taken to fix the problem, and what was the result?
I'm trying to implement a map function for fixing the dead zones of motors. i think that most of my problems are connected to motor control.

Do you understand every part of the programs you downloaded?
Yes. As i said i have a good understanding of every single component and also i studied PID and Arduino's PID Library. However i feel that when i combine all together I mess up with something.

11

You should put print statements in the program to inform you of the values being input to the PID function, and output by it.

If you hold the robot upright, input and output should both be about zero.

If you tilt the robot, the input should be the correct tilt value, and the output should be large enough, and in the correct direction so that the motors move to correct the tilt.

If they aren't, that needs to be fixed first.

12

If you see my code I already did this. Angle measure it's good and also PID works. What i'm trying to do now is fixing dead zones of my motors. Let's make an example. PID give me an output of 40 on a scale of 255 for example but my motors at this rate don't move. They start to react around 60/70.

realoutput = map(abs(output), 0, 255, minAbsSpeed, 255)

With this line of code I shift the "zero" from 0 to minAbsSpeed (60/70 for me) and re-maps every single value in the new scale.

My print statements

    Serial.print("\t");
    Serial.print(input);
    Serial.print("\t");
    Serial.print(output);
    Serial.print("\t");
    Serial.println(realoutput);

Input and output are printed while realoutput no, but bot seems to react differently. I'm going to do some tests

13

If you see my code I already did this.

Your code does not provide any information about the tests you did, and what you concluded from them.

This is the first useful bit of information that you have provided:

PID give me an output of 40 on a scale of 255 for example but my motors at this rate don't move. They start to react around 60/70.

That motor behavior is partly due to the low battery voltage and use of the ancient, inefficient L298 motor driver. Also, your Kp is too small. If Kp were twice as large, the output would be 80 and the motors would move.

However you forgot to explain what input value produced the PID output of 40. Please post a short table of actual tilt values in degrees, the measured tilt values in degrees and the PID input and output values.

Finally: I see a lot of constants in the code that have not been changed from the version you downloaded. The "magic numbers" that someone else found, for their own equipment, WILL NOT WORK on your equipment.

    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(36);
    mpu.setYGyroOffset(-23);
    mpu.setZGyroOffset(20);
    mpu.setZAccelOffset(-60189); // 1688 factory default for my test chip
14

Hello Alessio1

I'm Looking at the communication with the MPU6050 attempting to determine how you are handling the data retrieval
the first thing to consider:

  // wait for MPU interrupt or extra packet(s) available
  while (!mpuInterrupt && fifoCount < packetSize)
  {
    //no mpu data - performing PID calculations and output to motors
    pid.Compute(); 

/*
if we don't have any data it is better not to calculat PID rather than make a calculation. 
Each time the PID calculates it modifies the output The Integral adds the error multiplied by Ki to the output and KD influence is copletely removed because there was no change from the last reading.
consider waiting another 10 miliseconds for the next good reading rather than placing old data back through the PID
*/

    //I think the problems are here


// Your Motor control code.

  }

Here is the example motor control code I posted a while ago
note that PowerOffset will need to be set at a point where the motors are about to move What is "Motor Minimums" in 60 seconds #PID #BalancingBot - YouTube
Set TurnVal to zero if your motors are balanced +- value can help keep thing moving straight
Power is zero when off and +- to set the direction
this code should help you with your motor responsiveness

void DirectDrive(int Power, int TurnVal ) {
  if (!Mode)return;
  bool DiredtionA = true; //true/false change to reverse motor A direction (Change if motor is backwards!)
  bool DiredtionB = true; //true/false change to reverse motor B direction (Change if motor is backwards!)
  TurnVal = 0 - TurnVal; // if turning in worng direction uncomment;
  static double LastPowerA;
  static double LastPowerB;
  TurnVal = (abs(TurnVal) < 10) ? 0 : TurnVal; // Stops jittering (Deadband)
  int PowerA = constrain(Power + TurnVal, 0 - (int)PIDOutMax, (int)PIDOutMax); // Any adjustment to power can't exceed +- PIDOutMax;
  int PowerB = constrain(Power - TurnVal, 0 - (int)PIDOutMax, (int)PIDOutMax); // Any adjustment to power can't exceed +- PIDOutMax;
  int OutA = map(abs (PowerA), 0, (int) PIDOutMax, MinPower - PowerOffset, 255); // Convert +- PIDOutMax to MinPower to 255 for PWM Motor output
  int OutB = map(abs (PowerB), 0, (int) PIDOutMax, MinPower + PowerOffset, 255); // Convert +- PIDOutMax to MinPower to 255 for PWM Motor output
 
 if ((PowerA == 0) || ((LastPowerA > 0) != (PowerA > 0))) {  // check for no power or change in direction Motor A

    CoastA();
  }
  if ((PowerB == 0) || ((LastPowerA > 0) != (PowerB > 0))) {  // check for no power or change in direction Motor B
    CoastB();
  }
  LastPowerA = PowerA;
  LastPowerB = PowerB;
  if (PowerA < 0) {
    HBridgeGoA(DiredtionA);
  }
  if (PowerA > 0) {
    HBridgeGoA(!DiredtionA);
  }
  if (PowerB < 0) {
    HBridgeGoB(DiredtionB);
  }
  if (PowerB > 0) {
    HBridgeGoB(!DiredtionB);
  }
  if (abs (PowerA) > 0) {
    SetPWMA(OutA);
    RunA();
  }
  if (abs (PowerB) > 0) {
    SetPWMB(OutB);
    RunB();
  }

}


void CoastA() {
  SetPWMA(0); // Coasting Zero Power
  HBridgeLockA(false);
}
void CoastB() {
  SetPWMB(0);// Costing Zero Power
  HBridgeLockB(false);
}
void RunA() {

}
void RunB() {

}


void FullBreaking () {
  HBridgeLockA(false);
  HBridgeLockB(false);
  SetPWMA(255);
  SetPWMB(255);
}

void SetPWMA(int Speed) {
  analogWrite(PWM_A, Speed);
}

void SetPWMB(int Speed) {
  analogWrite(PWM_B, Speed);
}

/* My HBridge configuration =
I have 2 1/2 H-Bridge chips per motor each chip has one High/Low side select input and an enable input.
The enable input of both 1/2 inputs are tied together and connected to pin 6 for Hbridge A and pin 3 for Hbridge B with PWM enabled
The following code focuses only on the High/Low side select
So by default when the enable input (PWM) is HIGH  The Low side of my HBridge is on.
Now I can enable the High side by connecting 5v to the Select pin (HBgA1HighEn pin 8,HBgA2HighEn pin 7 & HBgB1HighEn pin 4,HBgB2HighEn pin 5).
so for costing all I need to do is set PWM low.
and for full breaking I need to set all High/Low selects to the Low side and enable the H-Bridge (analogWrite(PWM_A, 255);)
Forward motion I put one side high and the other low reverse that and the motor turns in reverse.
Simple :)
*/

void HBridgeLockA(bool HighSide) {
  digitalWrite(HBgA1HighEn, HighSide);
  digitalWrite(HBgA2HighEn, HighSide);
}

void HBridgeLockB(bool HighSide) {
  digitalWrite(HBgB1HighEn, HighSide);
  digitalWrite(HBgB2HighEn, HighSide);
}

void HBridgeGoA(bool Reverse) {
  digitalWrite(HBgA1HighEn, Reverse);
  digitalWrite(HBgA2HighEn, !Reverse);

}

void HBridgeGoB(bool Reverse) {
  digitalWrite(HBgB1HighEn, Reverse);
  digitalWrite(HBgB2HighEn, !Reverse);
}

To be continued.
Z

15

I see you are using PID_v1
While this is excellent code for temperature control it is flawed when trying to use it for balancing bot.
The MPU6050 has a sample time of 10 MS
the PID _v1 also has a fixed time which may or may not match up with the interrupt trigger from the MPU6050
so if it matches then great if it doesn't then your PID calculation is faulty

Your code

    //setup PID
    pid.SetMode(AUTOMATIC);
    pid.SetSampleTime(10); // this is correct 
    pid.SetOutputLimits(-255, 255);

now if the interrupt matches the exact sample time of 10ms your calculations will work

I have eliminated the fixed time interval with my version of PID and it stabilized the balancing of my robot. as well as it made it easier to tune
I've attached the latest version of this code.
Note that you do not need integral to balance your bot. so eliminate that always have integral at ZERO

Here is the main sequence of my program

// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================
void loop() {
 
  // I've experienced several false starts where the MPU6050 fires Pich values < 1° at the program which triggers the motor to run and causes havic with my bot! So i added StartUP and trash the frist 100 readings.
  static int StartUP = 100; // lets get 100 readings from the MPU before we start trusting them (Bot is not trying to balance at this point it is just starting up.)
  if (mpuInterrupt ) { // wait for MPU interrupt or extra packet(s) available
    GetDMP(StartUP > 1); // if StartUP is true then GetDMP skipps the MPUMath()  and does not call the DirectDrive() preventing motors from engaging.
    StartUP = abs(StartUP - 1); // Prevents Startup from going negitive and rolling over to a positivew number.
  }
  if (!Mode && InputABS < 10) {
    Mode = true;
    myPID.SetMode(AUTOMATIC);
    myTurnPID.SetMode(AUTOMATIC);
  }

  static long StopTime;
}



void GetDMP(bool Startup) { // Best version I have made so far
//  Serial.println(F("FIFO interrupt at:"));
//  Serial.println(micros());
  static unsigned long LastGoodPacketTime;
  mpuInterrupt = false;
  FifoAlive = 1;
  fifoCount = mpu.getFIFOCount();
  if ((!fifoCount) || (fifoCount % packetSize)) { // we have failed Reset and wait till next time!
    digitalWrite(LED_PIN, LOW); // lets turn off the blinking light so we can see we are failing.
    mpu.resetFIFO();// clear the buffer and start over
  } else {
    while (fifoCount  >= packetSize) { // Get the packets until we have the latest!
      mpu.getFIFOBytes(fifoBuffer, packetSize); // lets do the magic and get the data
      fifoCount -= packetSize;
    }
    LastGoodPacketTime = millis();
    MPUMath(); // <<<<<<<<<<<<<<<<<<<<<<<<<<<< On success MPUMath() <<<<<<<<<<<<<<<<<<<
    digitalWrite(LED_PIN, !digitalRead(LED_PIN)); // Blink the Light
  }
}


void MPUMath() {
  mpu.dmpGetQuaternion(&q, fifoBuffer);
  mpu.dmpGetGravity(&gravity, &q);
  mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
  Input = (ypr[1] * 180 / M_PI) * 10;
  InputABS = abs(Input);
  if (InputABS > 450) { // 45.0 degrees
    Mode = false;
    myPID.SetMode(MANUAL);
    myTurnPID.SetMode(MANUAL);
    Output = 0;
    Turn = 0;
    FullBreaking ();
    return;
  }

  Setpoint = (SPZero * 10) + SPAdjust; // SPAdjust is for offsetting the balance for driving the robot
  myPID.Compute();
  
  if (TurnControlEnable) {
  YawMPU = (ypr[0] * 180 / M_PI) ;
  YawInput = YawMPU + YawOffset;
  if (YawInput > 180) YawInput -= 360; // rollover to negative
  if (YawInput < -180) YawInput += 360; // rollover to positive
  YawInput = YawInput;
  myTurnPID.Compute();
  } else Turn = 0;
  DirectDrive(Output, Turn);
}

The Balancing bot code that is attached is for my small bot shown in the video
Let me know what questions you have
Z

Balanceing_Bot_Works_.ino (17.5 KB)

PID_v2.cpp (8.08 KB)

PID_v2.h (2.94 KB)

16

@jremington

Thank you again for your response. I have bought another battery, so now i will have 11.1V. Hope this can help. What you call magical numbers are my calibration value, forgot to mention.

@zhomeslice
Thank you for sharing your code. I'm trying to understand your motor control and how i have to modify your sketch to fit my design.
In your opinion my motors and motor controller are fine?

17

Alessio1:
@zhomeslice
Thank you for sharing your code. I'm trying to understand your motor control and how i have to modify your sketch to fit my design.
In your opinion my motors and motor controller are fine?

Yes, balancing shouldn't take much power. Sloppy gears have more influence than motor power.
I shut my motors off once my bot falls past 45° you should do the same. Also, consider not activating the PID loop & motors until you are at +-1° or what would be vertical. this will help you tune your PID and start balancing with weak or small motors. I know I mentioned it before, avoid using Integral. I know I mentioned it before but it will only give you grief when starting. adding it later will help balance on sloped surfaces and uneven areas if you need to do that.
Z

18

If you want good response and no deadzone from the motors you should be using synchronous
rectification mode in the motor controller, not any sort of decay mode. This will mean the motors
are responsive at zero speed.

In synchronous rectification mode the H-bridge is always switched on, but will PWM between forwards
and backwards direction. With 50% duty cycle PWM this means no net movement, but at 49% or 51%
you get definite response.

The other approach is to use a motor driver with current sensing and close a current control loop
with a fast inner PID, this then being used by the balance loop. Current determines torque so this
is a force feedback system rather than a speed feedback system.

Good current control across the zero crossing tends to imply synch rect mode anyway...

Synch rect mode is great, but is often not supported as well as its less efficient for less demanding
applications of motors.

19

MarkT:
If you want good response and no deadzone from the motors you should be using synchronous
rectification mode in the motor controller, not any sort of decay mode. This will mean the motors
are responsive at zero speed.

In synchronous rectification mode the H-bridge is always switched on, but will PWM between forwards
and backwards direction. With 50% duty cycle PWM this means no net movement, but at 49% or 51%
you get definite response.
...

@Alessio I use the "synchronous rectification mode" in the example code I shared as mentioned by @MarkT.

MinPower is the minimum power needed to move the motor PWM will never go below MinPower as mentioned above.

I didn't have time before to simplify my Motor Drive code earlier so here you go.

void DirectDrive(int Power ) {
  bool Diredtion = true; //true/false change to reverse motor A direction (Change if motor is backwards!)
  static double LastPower;
  int OutA = map(abs (Power), 0, (int) PIDOutMax, MinPower, 255); // Convert +- PIDOutMax to MinPower to 255 for PWM Motor output
 
 if ((Power == 0) || ((LastPower > 0) != (Power > 0))) {  // check for no power or change in direction Motor A
    Coast();
  }

  LastPower = Power;
  if (Power < 0) {
    HBridgeGo(Diredtion);
  }
  if (Power > 0) {
    HBridgeGo(!Diredtion);
  }

  if (abs (Power) > 0) {
    SetPWM(Out);
  }
}
void Coast() {
  SetPWM(0); // Coasting Zero Power
  HBridgeLock(false);
}

void FullBreaking () {
  HBridgeLock(false);
  SetPWM(255);
}

void SetPWM(int Speed) {
  analogWrite(PWM_Output, Speed);
}

void HBridgeLock(bool HighSide) {
  digitalWrite(HBg1HighEn, HighSide);
  digitalWrite(HBg2HighEn, HighSide);
}


void HBridgeGo(bool Reverse) {
  digitalWrite(HBg1HighEn, Reverse);
  digitalWrite(HBg2HighEn, !Reverse);

}

Simplified Code Above for Equal driving of both motors.
Int Power is a number from +PIDOutMax to -PIDOutMax. I like to use a larger number like +-1000 to allow for a more detailed PID calculation. but +-255 can work. Just make it match PIDOutMax
When Power is set to Zero no output will be present a Positive number will drive the motor forward and a negative number will drive the motor in reverse.

As for connecting your motor controller:

HBg1HighEn = Pin ##; connect this pin to the H-Bridge pin IN1 AND IN3
HBg2HighEn = Pin ##; connect this pin to the H-Bridge pin IN2 AND IN4

PWM_Output = Pin ##; Must be a PWM pin and connect it to H-Bridge Pins ENA and ENB

How this works:
When driving forward IN1 will be High and IN2 will be low
The PWM signal will be injected into the ENA to pulse drive the HBridge
When driving Reverse IN1 will be Low and IN2 will be High
The PWM signal will be injected into the ENA to pulse drive the HBridge
Coasting is done by setting both sides of the H bridge High or Low and Not Enabling the H Bridge
Breaking is done by setting both sides of the H-Bridge High or Low and adding PWM to the Enable pin. full breaking Set the Enable Pin High.
The same thing is mirrored for the second H-Bridge.

Z

20

Thank you again zhomeslice, I'm going to test your code in the next days :slight_smile: