## Beginner - help with self-balancing robot build

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

### Re: Beginner - help with self-balancing robot build

Thanks, i'll have a study of this code today hopefully

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

### Re: Beginner - help with self-balancing robot build

Had a play with the final iteration of the driver

The code below gives me nice looking data, presuming i have things set correctly I added in settings from previous tests to try and not alter too much in one go.

The numbers are pretty stable in the 0.01deg range but there is a notable blip - it almost looks like a heartbeat which bumps the avg up by a factor of 10, using my loop below at 100ms its almost but not exactly every 10 cycles, odd but interesting none the less.

Code: Select all

``````import utime
from mpu6050 import MPU6050, FILTER_GYRO, FILTER_ANGLES, ANGLE_COMP, GYRO_FS_250, ACCEL_FS_2, DLPF_BW_42

# Setup the MPU6050 IMU
sda = 14
scl = 15
bus = 1
offsets=(-618, 3111, 1366, 69, -17, 118)  # coffee table
#offsets = (-2578, -4823, 1030, 93, 30, -41) # robot

config = dict(
gyro        = GYRO_FS_250,                 # Set gyro range to 250deg/sec
accel       = ACCEL_FS_2,                  # Set accelerometer range to 2g
dlpf        = DLPF_BW_42,                  # Set digital low pass filter to 42
rate        = 32,                          # MPU6050_SPLRTDIV ~ comp filter samples at half of this number
A           = 0.8,                         # Percent of accel data to use, 1-alpha is percent of gyro to use
filtered    = FILTER_GYRO | FILTER_ANGLES, # Wont filter accelerometer raw readings
anglefilter = ANGLE_COMP,                  # Apply only complimentary filter to angles
angles      = False                        # Send data to handler as angles
)

mpu = MPU6050(bus, sda, scl, ofs=offsets, **config)

roll_min = 0
roll_max = 0
t=0
if mpu.passed_self_test:
while True:
angles = mpu.angles

if (t > 25): #let it settle for 25 readings
if (angles.roll < roll_min): roll_min = angles.roll
if (angles.roll > roll_max): roll_max = angles.roll

print("Loop:{:3.0f}  Min:{:6.2f}  Max:{:6.2f}  Cur:{:6.2f}".format(t , roll_min, roll_max, angles.roll))

t+=1

utime.sleep_ms(100)

``````

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

### Re: Beginner - help with self-balancing robot build

Still reading, found this site today that actually goes into a little detail....
https://people.ece.cornell.edu/land/cou ... index.html

It's lengthy so don't rush to read it From what i have read so far, my build is the same physical size, it uses a PIC32 MCU and not Arduino for a change, the same IMU, PWM motor driver but does use BDC gear motors where I have steppers - I'm not too concerned here as i know for a fact that steppers do work as there are plenty of other builds running NEMA17's

Steppers have the desired torque at low speed but do need ramping or you risk losing steps, not insurmountable but a consideration.

The big boost is that, with the considerable help from OneMadGipsy (thanks again) I now have a massive part of the donkey work in place - I have filtered and accurate data coming from the IMU, with a host of filter options and all easily configurable - I have a feeling this MP6050 driver will be very popular once it gets out there

The block i am missing is the PID control - I am now focussing on this. I have used/tuned PID before but only on a CNC mill with AC servos, still, I think the theory is constant. Just need to figure how and where to insert it into some code

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

### Re: Beginner - help with self-balancing robot build

Filling in some blanks, from testing it seems a stepper range of 50 to 4000Hz should be plenty, that equates to a linear speed of 0.76 to 61m/min - probably way too wide ranging but it's a start. I managed to get the little motors and pico up to about 8000Hz + and would have gone more.

That range was achieved with rudimentary ramping, going from 50 to 4000Hz took 131mS - no idea if that is aggressive enough or not yet, but I do know ramping is essential for steppers.

I think a max tilt of 25deg is plenty, its a pretty steep lean in reality.

Thats filled a few gaps, next I need to come up with a motor control routine that can take a speed and ramp from where it is to where it should be, up or down.......

Posts: 326
Joined: Wed Apr 28, 2021 1:57 am
Location: New Orleans, Louisiana
Contact: Website

### Re: Beginner - help with self-balancing robot build

__pid is the penultimate method in the MPU6050 script. Right before it is __calibrate. You have an example PID and an example of how to use it. The current PID is heavily specific to the MPU6050. Everywhere it has a condition with 0x3B, and is referring to reading, it is reading either the accelerometer or gyroscopes current measurement. Everywhere it has a condition with 0x06 and 0x13, and is referring to saving, it is saving it's current offset guesstimate respective to the 0x3B condition. The rest of it is the math that is necessary to make that guess for the MPU6050, and the loops necessary to make that guess a lot of times.
"Focus is a matter of deciding what things you're not going to do." ~ John Carmack

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

### Re: Beginner - help with self-balancing robot build

Anyone have a minute to give me some pointers in tidy / smart coding ??

I have a lump of code I wrote but its in my generally clumsy style I am trying to find out ways to do it that are more sensible, I'm sure there are many.

What it does....

It takes a speed value in range from -3000 to +3000 and sets PWM frequency accordingly, PWM can only be positive
A value of zero stops the PWM (duty_cycle= 0), (not added yet)
The motor usable range is 50 to 3000, anything from 1 to 49 would be 50
Values >0 will set the direction forwards, values <0 will be backwards
It will ramp the frequency up from current to target
To change direction it must ramp down then change then ramp up

So far it seems ok from some manual testing but are there better ways to do this that does not need me to have a degree in advanced coding

Code: Select all

``````def motors(speed_in):
global targetspeed
global currentspeed
targetspeed = speed_in

if (targetspeed > 0 and currentspeed = 0):                      # Go forwards from stopped
if (targetspeed < minspeed): targetspeed = minspeed          # Apply min speed
dir_left.value(fwd)
dir_right.value(fwd)
driveL.duty_u16(dRun)
driveR.duty_u16(dRun)
for ramp in range(currentspeed, targetspeed, 1):
driveL.freq(ramp)
driveR.freq(ramp)
utime.sleep_us(25)
currentspeed = targetspeed

if (targetspeed > 0 and currentspeed > 0):                      # Go forwards faster
if (targetspeed > currentspeed):                        # Speed up, same direction
for ramp in range(currentspeed, targetspeed, 1):
driveL.freq(ramp)
driveR.freq(ramp)
utime.sleep_us(25)
currentspeed = targetspeed
if (targetspeed < currentspeed):                        # Slow down, same direction
for ramp in range(currentspeed, targetspeed, -1):
driveL.freq(ramp)
driveR.freq(ramp)
utime.sleep_us(25)
currentspeed = targetspeed

if (targetspeed < 0 and currentspeed = 0):                      # Go backwards from stopped
if (targetspeed > minspeed): targetspeed = -minspeed         # Apply min speed
dir_left.value(rev)
dir_right.value(rev)
driveL.duty_u16(dRun)
driveR.duty_u16(dRun)
for ramp in range(currentspeed, targetspeed, -1):
driveL.freq(ramp)
driveR.freq(ramp)
utime.sleep_us(25)
currentspeed = targetspeed

if (targetspeed < 0 and currentspeed < 0):                      # Go backwards faster
if (targetspeed > currentspeed):                        # Slow down, same direction
for ramp in range(currentspeed, targetspeed, 1):
driveL.freq(abs(ramp))
driveR.freq(abs(ramp))
utime.sleep_us(25)
currentspeed = targetspeed
if (targetspeed < currentspeed):                        # Speed up, same direction
for ramp in range(currentspeed, targetspeed, -1):
driveL.freq(abs(ramp))
driveR.freq(abs(ramp))
utime.sleep_us(25)
currentspeed = targetspeed

if (targetspeed < 0 and currentspeed > 0):                  # Go backwards from going forwards
localspeed = abs(targetspeed)
for ramp in range(currentspeed, minspeed, -1):          # Slow to min speed or we lose steps
driveL.freq(abs(ramp))
driveR.freq(abs(ramp))
utime.sleep_us(25)
currentspeed = minspeed
dir_left.value(rev)                                         # Change direction
dir_right.value(rev)
for ramp in range(currentspeed, localspeed, 1):        # Speed up to requested speed
driveL.freq(ramp)
driveR.freq(ramp)
utime.sleep_us(25)
currentspeed = targetspeed

if (targetspeed > 0 and currentspeed < 0):              # Go forwards from going backwards
localspeed = abs(currentspeed)
for ramp in range(localspeed, minspeed, -1):           # Slow to min speed or we lose steps
driveL.freq(ramp)
driveR.freq(ramp)
utime.sleep_us(25)
currentspeed = minspeed
dir_left.value(fwd)                                          # Change direction
dir_right.value(fwd)
for ramp in range(currentspeed, targetspeed, 1):        # Speed up to requested speed
driveL.freq(ramp)
driveR.freq(ramp)
utime.sleep_us(25)
currentspeed = targetspeed

``````

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

### Re: Beginner - help with self-balancing robot build

First comment would be you probably don't want those 'for' loops in there. You will be stuck in those until you come out, won't be able to react until they complete. There are ways round that but it soon gets messy. It's much easier to do it all on a master loop determining what you need to do for this iteration of the loop. Do that, then the next iteration.

I would likely have master loop of -

Code: Select all

``````while True:
DetermineTargetSpeed()
if target >= 0:                             # Wanting to go forward
if current >= 0 : IncreaseForwardSpeed()  #   Am going forward
else            : DecreaseBackwardSpeed() #   Am going backwards
else:                                       # Wanting to go backwards
if current < 0  : IncreaseBackwardSpeed() #   Am going backward
else            : DecreaseForwardSpeed()  #   Am going forwards
SetMotorSpeed(current)
``````
Abstracting things into their own modules rather having monolithic code will make things easier, more understandable for others and for yourself when you come back to it.

The main gain is that the above is understandable without needing to know how any of it is achieved.

It also makes it easier to write the four separate routines you need, for example ...

Code: Select all

``````def DecreaseForwardSpeed():
global current
if current > 0:
current = current - DecelerationFactor(current)
if current < 0:
current = 0

def IncreaseForwardSpeed():
global current
if current > 0:
current = current + AccelerationFactor(current)
if current > MAX_FORWARD_SPEED:
current = MAX_FORWARD_SPEED
``````
You can have "DecelarationFactor()" and "AccelerationFactor()" return 1 to start with, not care if it's perfect or not only that it sort of works. Get it working and tweak it later.

The easiest way to eat an elephant is small bites at a time.

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

### Re: Beginner - help with self-balancing robot build

Thanks hippy, appreciate that

I'm a little slow to start so forgive me The reason that i stopped and asked was that i could see it was getting unruly

So your idea is that there is no "ramp" as such - we just run it one step at a time and alter speed based on the current desire - either increase or decrease or stop ? All run from the main loop?

That way it can increase or decrease at will, one step at a time based on the target and current settings ??

I think I have that right?

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

### Re: Beginner - help with self-balancing robot build

Yes, that's the idea.

The acceleration factor added each step of the way defines the ramp; it could be fixed but that's not realistic, it could be a percentage increase, an exponential increase, whatever takes it from current speed to next speed without stalling, slipping, exploding.

One can think of it as a graph of maximum stepper motor speed against time, the adjustment at each step being whatever it takes to get to the next speed over whatever increment of time you have.

Posts: 326
Joined: Wed Apr 28, 2021 1:57 am
Location: New Orleans, Louisiana
Contact: Website

### Re: Beginner - help with self-balancing robot build

I said there wasn't going to be another version of my MPU6050 driver, but there actually had to be. I was polishing up all my docs, by even including some information from the datasheet tables in order to help people make better decisions, and I came across a problem. There was one part in my script that took it for granted that other people's math and logic is correct, but it wasn't. Once I realized the problem it cascaded a new issue throughout the entire script.

First let's talk about the initial problem. All these angle scripts tell you to divide gx and gy by 131. Well, that's only correct if you have fullscale range for the gyroscope set to 250. The default for my script is 500 so that number is automatically wrong. My script also allows you the opportunity to set the fullscale range to whatever you want so, the problems this creates can get a lot worse depending on what you set it to. Changing that problem to be correct only required me to change one line, but it led to another math solution that I took for granted, which was also wrong. It took two lines to fix that, and then all of the initial problem was fixed.

Now let's talk about the cascading problem. Since the initial problem was no longer a static number, if you decided to use any of my setters after instancing the driver, all of your calibrations would be wrong. Allowing the setters to stick around would be silly, because every setter would have to completely reinit AND recalibrate the device, but only if they are called from the front-end and not the back-end, as part of init is calling all of the setters. That's a big fart to stick right in the middle of whatever you're doing. Clock was the only thing that had a setter but did not have a constructor argument, so when I made all the setters private, I had to add clock to the constructor arguments. Whatever device configuration values you intend to use have to be declared before you run calibration, otherwise your calibration will be wrong. This isn't an issue I added, this is just how it is. These are the values that you HAVE to set before calibration and leave the same after calibration if you do not intend to use the defaults ~ clock, gyro, accel, dlpf and rate.

Once all of that was done, it made sense to reorganize the constructor one last time. Now the constructor can be viewed as groups in this order MPU6050(I2C, offsets, FIFO, device configuration, filters, misc/ignorable). Due to not having the setters public anymore it was necessary to write the docs in a different way. The setters part of the docs included the information for all of the constants, since that's exactly what the constants were for. Having to make such a dramatic change provoked me to practically rewrite the entire docs. It is much more comprehensive than it was before, and I felt like it was pretty damn comprehensive already.

I am positive this is my last go with this. I am positive of that this time because, while I was including some of the device docs in my docs I went over every last method and property in my code and compared it to the system specs AND register definitions. Every formula, every used register, every bit of logic. I took nothing for granted and actually just assumed that anything I used that came from somebody else was probably wrong. I'm not necessarily happy about the fact that I had so much more work to do on this. Especially since it all stemmed from literally 1 number. However, having this realization and making these changes has made the device readings even more accurate, which isn't surprising since 3 simple math problems were wrong. I'm never porting somebody else's stuff ever again. I've never done it that way before and I don't have these problems. I tend to do just fine with my own brain and a datasheet.
"Focus is a matter of deciding what things you're not going to do." ~ John Carmack

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

### Re: Beginner - help with self-balancing robot build

I'm never porting somebody else's stuff ever again.

Famous last words maybe ???

TBH I think its amazing the amount of effort you have put into this

Posts: 326
Joined: Wed Apr 28, 2021 1:57 am
Location: New Orleans, Louisiana
Contact: Website

### Re: Beginner - help with self-balancing robot build

TBH I think its amazing the amount of effort you have put into this
Yeah? How about throwing my repo a star if you feel that way and find my script helpful.
"Focus is a matter of deciding what things you're not going to do." ~ John Carmack

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

### Re: Beginner - help with self-balancing robot build

Mon Jun 14, 2021 11:00 pm
TBH I think its amazing the amount of effort you have put into this
Yeah? How about throwing my repo a star if you feel that way and find my script helpful.
Done

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

### Re: Beginner - help with self-balancing robot build

hippy wrote:
Mon Jun 14, 2021 7:01 pm
Yes, that's the idea.

The acceleration factor added each step of the way defines the ramp; it could be fixed but that's not realistic, it could be a percentage increase, an exponential increase, whatever takes it from current speed to next speed without stalling, slipping, exploding.

One can think of it as a graph of maximum stepper motor speed against time, the adjustment at each step being whatever it takes to get to the next speed over whatever increment of time you have.
Getting it so far but the acceleration factor - the loop time is unknown and not fixed, I guess it can't until the whole program is written and running ?

But at least this way its easier to play with numbers etc

Usually a trapezoidal ramp is good enough so thats a linear increase over time?
rlFXW.gif (3.51 KiB) Viewed 573 times

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

### Re: Beginner - help with self-balancing robot build

Ok, so i'm struggling a little here, been playing since 5:45 this morning but I'm not sure i'm winning much.

My test scribble is below, I can make it ramp up from say zero to 3000 nicely and also down from say 3000 to 100 or zero ok The code is a mess - its deep in the "what if" state, apologies

But my head, and the code, explodes when i hit negative numbers, this is my weak spot in thinking it seems I just can't grasp going from positive to negative, I have the motor part ok and it will take either sign then set direction and speed which seems ok but the ramping from positive to negative is fubar

Have i missed a trick?

Code: Select all

``````from machine import Pin, PWM
import utime

# Set some values
FWD = 1
REV = 0
D_RUN = 5000 #32767=50% duty cycle with 500us pulsewidth, 10000=15% & 150us, 5000=7.5% & 76us
D_STOP = 0
MAX_SPEED = 3000 # Motor frequency limit
MIN_SPEED = 30
global targetspeed
global currentspeed
currentspeed = 0
targetspeed = 0

# Define the drive Pins, drive PWM objects, set zero duty cycle to start with
dir_left = machine.Pin(11, machine.Pin.OUT)
dir_right = machine.Pin(13, machine.Pin.OUT)
drivePinR = machine.Pin(12)
drivePinL = machine.Pin(10)
driveL = PWM(drivePinL)
driveR = PWM(drivePinR)
driveL.duty_u16(D_STOP)
driveR.duty_u16(D_STOP)

button_run = machine.Pin(17, machine.Pin.IN, machine.Pin.PULL_DOWN)
button_dir = machine.Pin(16, machine.Pin.IN, machine.Pin.PULL_DOWN)

def IncreaseSpeed():
global currentspeed
#currentspeed += AccelerationFactor(currentspeed)
currentspeed = AccelerationFactor(currentspeed)
print("increase")

def DecreaseSpeed():
global currentspeed
#currentspeed -= AccelerationFactor(currentspeed)
currentspeed = DecelerationFactor(currentspeed)
print("decrease")

def AccelerationFactor(current):
if (current >= 0):
else:
current -= 100
return(current)

def DecelerationFactor(current):
if (current >= 0):
current -= 100
else:
current += 100
return(current)

def SetMotorSpeed(speed):
if (speed < 0):                           # Set direction
dir_left.value(REV)                   # Backwards
dir_right.value(REV)
else:
dir_left.value(FWD)                   # Forwards
dir_right.value(FWD)

speed = abs(speed)                        # Remove sign of speed value

if (speed > MAX_SPEED): speed = MAX_SPEED # Limit top speed

if (speed > 0 and speed < MIN_SPEED): speed = MIN_SPEED # Limit bottom speed

if (speed > 0):
driveL.freq(speed)                    # Set motor speed
driveR.freq(speed)
driveL.duty_u16(D_RUN)                # Enable or disable PWM
driveR.duty_u16(D_RUN)
else:
driveL.duty_u16(D_STOP)
driveR.duty_u16(D_STOP)

while True:

if (targetspeed > currentspeed):
IncreaseSpeed()                       # Move faster
elif (targetspeed < currentspeed):
DecreaseSpeed()                       # Move slower

#     if targetspeed >= 0:                             # Wanting to go forward
#       if currentspeed >= 0 : IncreaseForwardSpeed()  #   Am going forward
#       else                 : DecreaseBackwardSpeed() #   Am going backwards
#     else:                                            # Wanting to go backwards
#       if currentspeed < 0  : IncreaseBackwardSpeed() #   Am going backward
#       else                 : DecreaseForwardSpeed()  #   Am going forwards

if (targetspeed != currentspeed): print(targetspeed, currentspeed)

SetMotorSpeed(currentspeed)                      # Set current desired speed

if (button_run.value() == 1):
targetspeed = 3000

if (button_dir.value() == 1):
targetspeed = -100

utime.sleep_us(1000)

``````

Posts: 326
Joined: Wed Apr 28, 2021 1:57 am
Location: New Orleans, Louisiana
Contact: Website

### Re: Beginner - help with self-balancing robot build

What is the max forward and backward numbers that spins the motor in those directions?
"Focus is a matter of deciding what things you're not going to do." ~ John Carmack

Posts: 326
Joined: Wed Apr 28, 2021 1:57 am
Location: New Orleans, Louisiana
Contact: Website

### Re: Beginner - help with self-balancing robot build

Maybe my last question could be asked better.

How do you determine the direction the motor spins?

If it's by some trigger, what is the trigger? If it's by value, what is the maximum forward value, and maximum backward value?

Give me those pieces of information and I could write a perfect ramp for either direction... probably all in one formula and you don't need all this if one way else the other way stuff.

Look at the game controller code I posted under player.set_position. There are no conditions. It's just some simple addition and division in min/max constraints. More tilt increases speed and less reduces it. The direction is built in to the sign of the angle. What you are trying to do is just that with a ramping of values added. You could probably handle both directions in 3 or 4 lines of code. If you give me the answers I requested I'll prove it.
"Focus is a matter of deciding what things you're not going to do." ~ John Carmack

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

### Re: Beginner - help with self-balancing robot build

Tue Jun 15, 2021 12:12 pm
Maybe my last question could be asked better.

How do you determine the direction the motor spins?

If it's by some trigger, what is the trigger? If it's by value, what is the maximum forward value, and maximum backward value?

The direction is based on the sign of the speed figure.

This is all presuming my reading that PID being fed by an IMU will output target numbers that can be negative or positive as the angles being fed in are also negative or positive.

The ultimate target is of course zero angle i.e. vertical (or some preset number)

The motors are driven by a controller that takes a pulse train from PWM, and a direction pin, PWM can only go down to about 10Hz but thats way too low to use so i guess a range of 50 to 3000Hz is good based on observations and speed calcs.

It can ramp pretty fast but cannot go zero ramp, my tests seem happy with a time of about 100-130ms or so to go from 50 to 3000, could probably be pushed harder but that will need real-world testing in motion.

Hope that helps

Posts: 326
Joined: Wed Apr 28, 2021 1:57 am
Location: New Orleans, Louisiana
Contact: Website

### Re: Beginner - help with self-balancing robot build

So, you're entire direction is just based on pulling a pin high or low, but otherwise all of the rest of the numbers are the same? I'll be back in a few minutes with your answer.
"Focus is a matter of deciding what things you're not going to do." ~ John Carmack

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

### Re: Beginner - help with self-balancing robot build

Tue Jun 15, 2021 12:44 pm
So, you're entire direction is just based on pulling a pin high or low, but otherwise all of the rest of the numbers are the same? I'll be back in a few minutes with your answer.
Exactly

It just changes direction based on the speed swinging pos or neg, naturally the direction can only be changed within the minimum speed deadband = -50 to 50

Posts: 326
Joined: Wed Apr 28, 2021 1:57 am
Location: New Orleans, Louisiana
Contact: Website

### Re: Beginner - help with self-balancing robot build

Here ya go. I'm sure this isn't going to "just work" but it gets you in the pocket. I didn't take into consideration that the motors shouldn't just reverse directions on a whim. You'll have to do something about that.

Code: Select all

``````from mpu6050 import *
from machine import Pin, PWM

_MAXSPEED = const(3000)
_MINSPEED = const(30)
_MAXPITCH = const(30) #you probably need to adjust this
_RAMP     = const(100)
_RUN      = const(5000)
_STOP     = const(0)
_L_DIRPIN = const(11)
_L_DRIVE  = const(10)
_R_DIRPIN = const(13)
_R_DRIVE  = const(12)
_BTN_RUN  = const(17)
_BTN_DIR  = const(16)
_I2C_BUS  = const(0)    #change this to the proper pin numer
_I2C_SDA  = const(0)    #change this to the proper pin numer
_I2C_SCL  = const(0)    #change this to the proper pin numer

class Robot(object):
def __init__(self):

self.__Ldir = Pin(_L_DIRPIN, Pin.OUT, Pin.PULL_DOWN)
self.__Rdir = Pin(_R_DIRPIN, Pin.OUT, Pin.PULL_DOWN)
self.__Lpwm = PWM(Pin(_L_DRIVE, Pin.OUT))
self.__Rpwm = PWM(Pin(_R_DRIVE, Pin.OUT))
self.__Lpwm.init(0, 0)                                #i just wrote whatever here, this is possibly incorrect freq=0, duty=0
self.__Rpwm.init(0, 0)                                #same

self.__btn_run = Pin(_BTN_RUN, Pin.IN, Pin.PULL_DOWN)

# I didn't make this button happen
# In this version the robot should just try to balance
self.__btn_dir = Pin(_BTN_DIR, Pin.IN, Pin.PULL_DOWN)

self.__targetspeed  = 0
self.__currentspeed = 0
self.__speed_diff   = 0

#REMEMBER: If you use this rate, dlpf and/or gyro, you have to calibrate with these values first
cfg = dict(
offsets     = (1292, -1613, 406, 37, 4, 58)
rate        = 20           ,
dlpf        = DLPF_BW_42   ,
gyro        = GYRO_FS_250  ,
filtered    = FILTER_ANGLES,
anglefilter = ANGLE_COMP   ,
)
self.__mpu = MPU6050(_I2C_BUS, _I2C_SDA, _I2C_SCL, **cfg)

while True:
if self.__btn_run.value():
self.__Lpwm.duty_u16(_RUN)
self.__Rpwm.duty_u16(_RUN)
pitch = self.__mpu.angles[1]            #get pitch
self.__Ldir.value(pitch >= 0)           #set direction pin high or low based on pitch value
self.__Rdir.value(pitch >= 0)           #same
pitch = abs(pitch)                      #dump sign, if any
self.__targetspeed    = int(_MAXSPEED/_MAX_PITCH*pitch)
self.__speed_diff     = self.__target_speed - self.__current_speed
self.__current_speed += min(abs(self.__speed_diff), _RAMP) * (abs(self.__speed_diff)/self.__speed_diff)
self.__Lpwm.freq(self.__current_speed)
self.__Rpwm.freq(self.__current_speed)
else:
self.__Lpwm.duty_u16(_STOP)
self.__Rpwm.duty_u16(_STOP)
self.__Lpwm.freq(0)
self.__Rpwm.freq(0)

Robot()
``````
"Focus is a matter of deciding what things you're not going to do." ~ John Carmack

Posts: 326
Joined: Wed Apr 28, 2021 1:57 am
Location: New Orleans, Louisiana
Contact: Website

### Re: Beginner - help with self-balancing robot build

The below is the only part that should need any explanation. This code hasn't been tested in any way. I didn't even run it to make sure there weren't any syntax errors. I can't because I have a lot of stuff on my pico that is using the exact same pins you are in a completely different way. I'm confident that any error (if any) is some minor syntax issue, and that my logic is sound. However, regardless of how correct the logic may be, I have no doubt that you will need to adjust the numbers it's using.

Code: Select all

``````
self.__targetspeed    = int(_MAXSPEED/_MAX_PITCH*pitch)
self.__speed_diff     = self.__target_speed - self.__current_speed
self.__current_speed += min(abs(self.__speed_diff), _RAMP) * (abs(self.__speed_diff)/self.__speed_diff)
``````

self.__targetspeed = int(_MAXSPEED/_MAX_PITCH*pitch)
this sets the target speed based on the pitch, it assumes 3000 is enough to rectify it at 30 degrees. Basically, this formula will add 100 to the speed for every degree of pitch.

self.__speed_diff = self.__target_speed - self.__current_speed
This just finds out what the difference is between the target and current speed. If the current speed is too high this will be negative, which is perfectly fine for the next line

self.__current_speed += min(abs(self.__speed_diff), _RAMP) * (abs(self.__speed_diff)/self.__speed_diff)
first we find out if our remaining speed difference is less than our ramp value. Let's assume it is. Let's assume it is -62. abs(-62) is 62. then abs(-62)/-62 is -1. -1*62 is -62. For these numbers to be true it must mean our current speed is 62 too high.Let's assume our target speed is 100. That means our current speed must be 162. 162 + -62 = 100 Voila' you've just reduced speed using one simple formula that will also increase it if the difference is positive. You may ask: "Well if you put in -62, why did you have to do all that to get -62 out?" Simple, what if the number was -162? That formula would return -100 ... the max ramping to decrease by. We do all that work to figure out if we are going to use the ramp or what's left in the difference, and then we figure out if it's positive or negative. POS + POS = addition POS + NEG = subtraction.

That's my 3 line solution. All the rest of what I gave you was just structure for your code. This is the meat and potatoes part.
Last edited by OneMadGypsy on Tue Jun 15, 2021 2:44 pm, edited 1 time in total.
"Focus is a matter of deciding what things you're not going to do." ~ John Carmack

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

### Re: Beginner - help with self-balancing robot build

Tue Jun 15, 2021 2:04 pm
Here ya go. I'm sure this isn't going to "just work" but it gets you in the pocket. I didn't take into consideration that the motors shouldn't just reverse directions on a whim. You'll have to do something about that.
Thanks OMG, Its gonna take me a while to soak that up, I'm not used to your ultra compact coding, mine tends to be more bloat than code i guess Its a throwback to only having MS VB6 as a coding history I'm afraid, things were simpler back then

For example, if I did this to the code, would it get rid of the use of a button and replace it with only allow action when the tilt is between -MAXPITCH and MAXPITCH ????

Code: Select all

``       if self.__mpu.angles[1] in range(-_MAXPITCH, _MAXPITCH):            #not fallen over``

Code: Select all

``if self.__btn_run.value():``

Posts: 326
Joined: Wed Apr 28, 2021 1:57 am
Location: New Orleans, Louisiana
Contact: Website

### Re: Beginner - help with self-balancing robot build

...between -MAXPITCH and MAXPITCH ????
yep

...ultra compact code

I used to write everything under it's own specific function that explains what it is. Then I learned math and stopped doing that. All this specific function stuff mostly just has you copy/pasting the same thing over and over with one tiny change, like > to <. There are plenty of programmers out there at various skill levels that will disagree with my way of doing it. I don't care. I'd rather write one slightly more complicated to read method that works in both directions than copy/paste the same method twice with one little change just to make it 5% more readable. Plus, the more you do it, that 5% get's smaller and smaller cause you end up writing the same formulas over and over from one project to the next. Those 3 lines will increase or decrease speed in either direction. I personally don't feel like 4 functions to do the same thing is any percent more readable. All the logic for every direction and function of a direction is one spot. And whereas the values for that formula may need tweaking, the formula itself shouldn't, so why do I need to read it, at all? Actually, if I wasn't trying to present this in a way that makes it easy for you to understand, my personal version would turn (ex)self.__speed_diff into self.__d, and the same goes for all those other long names.
Last edited by OneMadGypsy on Tue Jun 15, 2021 3:06 pm, edited 2 times in total.
"Focus is a matter of deciding what things you're not going to do." ~ John Carmack

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