## The perfect balancer robot :=)

Nitro_fpv
Posts: 110
Joined: Tue Mar 30, 2021 11:56 am
Location: Switzerland

### The perfect balancer robot :=)

Hi guys

Today I built a balancer robot from lying parts (unfortunately only a stepper motor had found).
The mechanics are catastrophic and unstable, the electronics clapped on the thing with adhesive tape, construction time: 1 hour.

But apparently the MicroPython code works perfectly.

Video:

Code:

Code: Select all

``````#This program may only be used for private non-commercial purposes.
#IMU Library:  https://github.com/micropython-IMU/micropython-bno055
#Have fun
#Pascal Heubi

from machine import Pin, I2C, PWM
from time import sleep
pwm = PWM(Pin(14)) #for the Stepper Driver
pwm.duty_ns(10000) #step value for Driver
richtung = Pin(15, Pin.OUT) #direction for Driver
freigabe = Pin(16, Pin.OUT) #Driver activ
freigabe.value(0) #Driver deaktiviert
sleep(1) #waiting for IMU

# Interpolates the value from the PID controller for the PWM output
def interpoliert(x, i_m, i_M, o_m, o_M):
return max(min(o_M, (x - i_m) * (o_M - o_m) // (i_M - i_m) + o_m), o_m)

from bno055 import *                            #IMU
i2c = I2C(0, scl=Pin(5), sda=Pin(4), freq=4000) #IMU
imu = BNO055(i2c)                               #IMU

KP = 36 #KP (P Regler)
KD = 36 #KD (D Regler)
KI = 8  #KI (I Regler)
ealt = 0      #Variable deviate
intergral = 0 #Variable deviate
schwerpunkt = -2.82 #Calibrate the center of gravity in deg

while True:
euler_imu = ('{2}'.format(*imu.euler())) #Euler Yaw
euler_imu = float (euler_imu) #Conversion to float
error = euler_imu + schwerpunkt #Calibrate the center of gravity
intergral = intergral + error #for I Regler
balance_output = error * KP + intergral * KI + (error - ealt) * KD   #PID Controller
if balance_output > 0:
pwm_servo = (interpoliert(balance_output, 0, 1000, 20, 15000))  #Interpolates PD controller - PWM
richtung.value(0)                                               #direction for Driver
else:
pwm_servo = (interpoliert(balance_output, 0, -1000, 20, 15000)) #Interpolates PD controller - PWM
richtung.value(1)                                               #direction for Driver
freigabe.value(1) #Activate the stepper if the angle is smaller than + - 20 °
if euler_imu > +20 : freigabe.value(0) #Deactivates the stepper at angles > +20 °
if euler_imu < -20 : freigabe.value(0) #Deactivates the stepper at angles < -20 °
pwm_servo = int (pwm_servo) #Conversion to int
pwm.freq(pwm_servo) #for the Stepper Driver
ealt = error #for D Regler
``````

Your opinion on this code interests me!
Of course, also the opinion on this quick shot project.

hippy
Posts: 12485
Joined: Fri Sep 09, 2011 10:34 pm
Location: UK

### Re: The perfect balancer robot :=)

Nitro_fpv wrote:
Sat Jun 19, 2021 1:54 pm
Your opinion on this code interests me!
Of course, also the opinion on this quick shot project.
Fantastic work. I was wondering when someone who knows more about doing it than those of us who don't would chime in.

I haven't studied your code but it looks as simple as I would have expected it to be. I won't pretend to understand control theory, PID and the maths involved but it does seem to confirm the simple principle of 'read the tilt, calculate the response, execute that response' I had imagined. In fact I would probably say it was simpler than I had imagined it to be - perhaps that is an advantage of knowing what you are doing, having experience and knowledge ?

If you did throw the code together having never done self-balancing bots before that is an even more deserved "well done".

And thanks for demonstrating that a single motor is enough.

If someone could create an MPU6050 driver which delivers the same or similar tilt value as the BNO055 I presume that could simply be dropped into your code, wouldn't be too much of a problem so long as the two are proportional to each other.

Nitro_fpv
Posts: 110
Joined: Tue Mar 30, 2021 11:56 am
Location: Switzerland

### Re: The perfect balancer robot :=)

hippy wrote:
Sat Jun 19, 2021 3:01 pm
Fantastic work. I was wondering when someone who knows more about doing it than those of us who don't would chime in.

I know my way around technology and physics, but very badly with MicroPython.
But I'm learning, also thanks to you and the others here who have helped me.

In fact, I'm not getting anywhere at the moment, I would like to add a second control loop to hold the position and control the speed in the code.
Unfortunately I can't find a solution to count the steps that I output to pin 15 using PWM.
I wanted to connect pin 15 to another pin and then use the PWM hardware to count the steps (pulses).

How could I do this?

Greetings Pascal

Nitro_fpv
Posts: 110
Joined: Tue Mar 30, 2021 11:56 am
Location: Switzerland

### Re: The perfect balancer robot :=)

It's typical again

I wrote and tested the balancer code within 4 hours.
Now I have spent another 6 hours trying to find a solution how to count steps and still haven't found a solution.

Nitro_fpv
Posts: 110
Joined: Tue Mar 30, 2021 11:56 am
Location: Switzerland

### Re: The perfect balancer robot :=)

Is there something like that in MicroPython?

hippy
Posts: 12485
Joined: Fri Sep 09, 2011 10:34 pm
Location: UK

### Re: The perfect balancer robot :=)

Nitro_fpv wrote:
Sat Jun 19, 2021 4:44 pm
Unfortunately I can't find a solution to count the steps that I output to pin 15 using PWM.
I wanted to connect pin 15 to another pin and then use the PWM hardware to count the steps (pulses).
PIO or Callback on Pin Edge Trigger is the solution there. You don't even need to link to another pin; you can use the output pin as an input pin; the trick is getting the order right.

This occasionally drops interrupts because of garbage collection - The MicroPython guys are looking into that.

Code: Select all

``````from   machine import Pin, PWM
import time

PWM_PIN = 15

count = 0
def Irq(arg):
global count
count += 1

inp = Pin(PWM_PIN, Pin.IN)
inp.irq(trigger=Pin.IRQ_FALLING, handler=Irq)

pwm = PWM(Pin(PWM_PIN, Pin.OUT))
pwm.freq(4000)
pwm.duty_u16(0x8000)

while True:
count = 0
time.sleep_ms(100)
counted = count
print(counted)
``````
Apologies for being distracted and not seeing your post sooner.

Nitro_fpv
Posts: 110
Joined: Tue Mar 30, 2021 11:56 am
Location: Switzerland

### Re: The perfect balancer robot :=)

Hello Hippy

Thanks for the example, I tested it and it worked, I had to increase the frequency to 15000.
Tomorrow I will build this into the Robi Code.

And really, you don't have to apologize for posting a solution too late!
thanks

Gavinmc42
Posts: 6755
Joined: Wed Aug 28, 2013 3:31 am

### Re: The perfect balancer robot :=)

Next step, find another stepper motor?
Steerable balance bot.

Very impressed.
I'm dancing on Rainbows.
Raspberries are not Apples or Oranges

Nitro_fpv
Posts: 110
Joined: Tue Mar 30, 2021 11:56 am
Location: Switzerland

### Re: The perfect balancer robot :=)

Gavinmc42 wrote:
Sun Jun 20, 2021 12:32 am
Next step, find another stepper motor?
Steerable balance bot.

Very impressed.

thanks

Yes exactly, is in planning
- Position and speed control
- Bluetooth control with mobile phone
- Second stepper and good chassis
....

davek0974
Posts: 305
Joined: Mon Jul 22, 2019 1:52 pm

### Re: The perfect balancer robot :=)

Very nicely done, congratulations,

I have been struggling for weeks to do this.

I will go and hide under a rock I think

davek0974
Posts: 305
Joined: Mon Jul 22, 2019 1:52 pm

### Re: The perfect balancer robot :=)

A quick question,

at this point...

Code: Select all

``````while True:
euler_imu = ('{2}'.format(*imu.euler())) #Euler Yaw
euler_imu = float (euler_imu) #Conversion to float
error = euler_imu + schwerpunkt #Calibrate the center of gravity``````
are you dealing with just degrees in one axis?

If so then it seems at first glance what i have goofed up is scaling of numbers and proper PID ??
The PWM is very similar i think and there is very little left

Nitro_fpv
Posts: 110
Joined: Tue Mar 30, 2021 11:56 am
Location: Switzerland

### Re: The perfect balancer robot :=)

Hello davek0974

Yes, here I read the angles (in absolute degrees) and include a correction that I have determined once.
Because for some reason my chassis is not completely symmetrical and the center of gravity is not 100 ° above the axis. : =)

Interpolating is also important in the code, since you can arbitrarily scale two input numbers into two output numbers.
In this case the PID output is converted to match the PWM output for the stepper.

davek0974
Posts: 305
Joined: Mon Jul 22, 2019 1:52 pm

### Re: The perfect balancer robot :=)

Nitro_fpv wrote:
Sun Jun 20, 2021 11:33 am
Hello davek0974

Yes, here I read the angles (in absolute degrees) and include a correction that I have determined once.
Because for some reason my chassis is not completely symmetrical and the center of gravity is not 100 ° above the axis. : =)

Interpolating is also important in the code, since you can arbitrarily scale two input numbers into two output numbers.
In this case the PID output is converted to match the PWM output for the stepper.

Thanks

davek0974
Posts: 305
Joined: Mon Jul 22, 2019 1:52 pm

### Re: The perfect balancer robot :=)

Hi again,
is there any chance you could list the motor and drive used please?

My reason is that (from my own thread) I am getting nowhere in building one of these, and having tried your code i am still getting nowhere - this tells me i have other serious issues here i think than just code.

I am currently trying the MPU6050 but it is filtered and gives good readings in a static test, it's also calibrated.

If your 'bot is held static in a stand say, does the motor run? Mine runs erratically every time.

Maybe my motors are no good for this use?? They are Nema17 type 17HS4401S, the wheels are 65mm diameter.

Any help would be very much appreciated.

hippy
Posts: 12485
Joined: Fri Sep 09, 2011 10:34 pm
Location: UK

### Re: The perfect balancer robot :=)

davek0974 wrote:
Mon Jun 21, 2021 10:52 am
If your 'bot is held static in a stand say, does the motor run? Mine runs erratically every time.
I am sure Nitro-fpv will answer but my observations, when it was standing upright of its own accord at the start of the video, there were single steps, maybe more, back and forth, but it didn't run out of control.

davek0974
Posts: 305
Joined: Mon Jul 22, 2019 1:52 pm

### Re: The perfect balancer robot :=)

hippy wrote:
Mon Jun 21, 2021 1:43 pm
davek0974 wrote:
Mon Jun 21, 2021 10:52 am
If your 'bot is held static in a stand say, does the motor run? Mine runs erratically every time.
I am sure Nitro-fpv will answer but my observations, when it was standing upright of its own accord at the start of the video, there were single steps, maybe more, back and forth, but it didn't run out of control.

Yes, i have observed that in the many dozens of videos i have watched - i simply cannot replicate that behaviour and i am at a complete loss why i can't.

Its just all over the place for some reason.

I would order a BNO055 but for these reasons...
1 - they seem to be out of stock all over, the ones that do have stock want silly money
2 - nearly every single SBR out there uses the MPU6050 - from tiny little ones to massive 6' tall ones.

hippy
Posts: 12485
Joined: Fri Sep 09, 2011 10:34 pm
Location: UK

### Re: The perfect balancer robot :=)

You could perhaps replace the MPU6050 reading routine with something which simply cycles through a list ... 0, 1, 2, 1, 0, -1, -2, -1, 0 ... and report what the response is, verify it's correct.

davek0974
Posts: 305
Joined: Mon Jul 22, 2019 1:52 pm

### Re: The perfect balancer robot :=)

hippy wrote:
Mon Jun 21, 2021 4:54 pm
You could perhaps replace the MPU6050 reading routine with something which simply cycles through a list ... 0, 1, 2, 1, 0, -1, -2, -1, 0 ... and report what the response is, verify it's correct.

Can do that, will try in the morning.

Nitro_fpv
Posts: 110
Joined: Tue Mar 30, 2021 11:56 am
Location: Switzerland

### Re: The perfect balancer robot :=)

davek0974 wrote:
Mon Jun 21, 2021 10:52 am
this tells me i have other serious issues here i think than just code.

seriously?
Before you write (or copy) a balance code, you should test the hardware and see whether your stepper motors are being controlled correctly.
Preferably with a test code.
I would say start with all the basics first, there are many tutorials about PI PICO, Stepper, Driver and Micropython.
But as I said, you have to sacrifice time for that!
Alternatively:
Copy an Arduino project, including the code.

I am now posting my stepper motor test code anyway:

Code: Select all

``````from machine import Pin, PWM
from time import sleep_ms, sleep
pwm = PWM(Pin(14)) #for the Stepper Driver
richtung = Pin(15, Pin.OUT)
freigabe = Pin(16, Pin.OUT) #Driver activ
pwm.duty_ns(10000)

while True:
freigabe.value(1)
richtung.value(0)
for dutyCycle in range (50, 15000, 4):
pwm.freq(dutyCycle)
sleep_ms(1)
sleep(1)
for dutyCycle in range (15000, 50 , -4):
pwm.freq(dutyCycle)
sleep_ms(1)
sleep(1)
richtung.value(1)
for dutyCycle in range (50, 15000, 4):
pwm.freq(dutyCycle)
sleep_ms(1)
sleep(1)
for dutyCycle in range (15000, 50 , -4):
pwm.freq(dutyCycle)
sleep_ms(1)
sleep(1)
freigabe.value(0)
Sleep(1)
``````

davek0974
Posts: 305
Joined: Mon Jul 22, 2019 1:52 pm

### Re: The perfect balancer robot :=)

thanks,

yes i tested the motors and drives at many different speeds and frequencies, i did this when figuring out the best way to set PWM up to drive the motor drivers.

I will try your test code tomorrow, believe me i have invested Plenty of time on this

davek0974
Posts: 305
Joined: Mon Jul 22, 2019 1:52 pm

### Re: The perfect balancer robot :=)

Hi again, any chance you could post details of your motor driver please?

I am using the A4988 which is step/dir input - is yours a full PWM control??

emma1997
Posts: 1737
Joined: Sun Nov 08, 2015 7:00 pm
Location: New England (not that old one)

### Re: The perfect balancer robot :=)

davek0974 wrote:
Mon Jun 21, 2021 1:53 pm
many dozens of videos i have watched - i simply cannot replicate that behaviour and i am at a complete loss why i can't.

Its just all over the place for some reason.
Following this thread and the other one for similar reasons, a suggestion might be to actually get one of the tarduino projects up and running first. Then, with some experience under your belt, port to the platform of choice. Nanos or Minis \$2-\$3 available everywhere and everything else the same. Having similar history myself and more recently observed over a dozen other individuals through similar process, suspect at least one or two orders magnitude less difficulty. IE ten hours instead of hundred or thousand.

IME struggling w/o knowing whether it's hardware or software issues can increase efforts big time. As hinted previously divide and conquer strategy can be very helpful reaching whatever goal.

On the other hand re-inventing wheels (lol) from scratch can be very educational too and of some value in it's own right. Even if only learning What Not to Do.

HermannSW
Posts: 5298
Joined: Fri Jul 22, 2016 9:09 pm
Location: Eberbach, Germany

### Re: The perfect balancer robot :=)

Nitro_fpv wrote:
Mon Jun 21, 2021 5:22 pm
seriously?
Before you write (or copy) a balance code, you should test the hardware and see whether your stepper motors are being controlled correctly.
Preferably with a test code.
I would say start with all the basics first, there are many tutorials about PI PICO, Stepper, Driver and Micropython.
But as I said, you have to sacrifice time for that!
Agreed, and for 1st balancing project use KISS principle (Keep It Stupid Simple) -- no PID controller needed!

Recently I started building Raspberry balancing robot that determined slope by analyzing camera frames and determining where laser dot appears in frame -- that is NOT KISS:
viewtopic.php?f=144&t=311514&p=1866691#p1863900

14 years ago I used simple 2wheels and half table tennis ball robot and teached it balancing (and driving) on two wheels. One wheel was always braked, so all movements were on a circle. Instead of doing perfect balancing in place, I made the robot slope too far to front, which resulted in robot driving forward circles on two wheels the whole time!

Control code for this was KISS:
If slope is below constant, move right wheel forward, otherwise backward (constant speed) -- that was all.

The robot (Asuro, German language Wikipedia article) had an IR transmission system (sender/receiver) for flashing C code onto the microcontroller. I moved that from side to bottom of robot to measure distance to floor -- and distance to floor is proportional to slope of robot.

P.S:
Braking one wheel is a nice trick to avoid runaway robot during development
This was runaway robot before braking one wheel -- luckily I was fast enough to catch robot before falling off the desk:

P.P.S:
I used same KISS control for doing pair run in kitchen for two Asuro robots:

Here distance to front was measured.
Right motor speed is always maximal.
If distance (to wall) is bigger than threshold, then left motor turns maximally forward, else maximally backward.
In youtube video there is not only sound additionally, but you can see that bigger robot passes the smaller on the left side once:
Btw, bigger and faster robot running parallel to slower robot on the left was achieved by different constants for KISS control algorithm, so faster robot had to drive bigger rounds.

I used another KISS algorithm for letting caterpillar robot do fast U-turn.
Both motors always run full speed, initially forward.
Then for U-turn, just reverse motor direction of left motor without reducing speed!
After some time reverse again:

(backward roll high in the air was unplanned)

Slow motion (90fps video played at 25fps, 3.6x slower than real):

https://stamm-wilbrandt.de/en/#robotics
https://hermann-sw.github.io/planar_graph_playground
https://stamm-wilbrandt.de/en#raspcatbt
https://github.com/Hermann-SW/memrun
https://github.com/Hermann-SW/Raspberry_v1_camera_global_external_shutter
https://stamm-wilbrandt.de/en/Raspberry_camera.html

davek0974
Posts: 305
Joined: Mon Jul 22, 2019 1:52 pm

### Re: The perfect balancer robot :=)

Thanks all,

I have a strong feeling this is a motor control issue i think - my first code was KISS, no pid, just bang-bang-bang, it failed, the second had more IMU control, that failed, the third has PID and failed.

They all failed the same way - erratic behaviour - the common part is the motors and motor drives.

I will be looking at Nitro_fpv's motor testing code tomorrow morning and will report in my own thread, apologies to Nitro for cluttering up his thread,

One thing is for certain - I now know 100% that what i desire can be done and it can be done with some very simple and easy to understand code. For this reason, I will not go the Arduino route simply because the goal is there in front of me and diverting to a new language and a new board is not going to help me reach that goal I feel.

davek0974
Posts: 305
Joined: Mon Jul 22, 2019 1:52 pm

### Re: The perfect balancer robot :=)

To clarify, I made a typo on a youtube post by Nitro_fpv and i will apologise for it now - i badly worded it and it can read like I was taking credit for the MPU6050 driver written by OneMadGypsy, It was totally not my intention and was purely a badly worded sentence, I apologise for any misunderstanding caused by it.