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

Re: Beginner - help with self-balancing robot build

Tue Jun 15, 2021 8:44 pm

OneMadGypsy wrote:
Tue Jun 15, 2021 6:57 pm
Hey, if you get sick of casting angles to int, drop this in the MPU6050 script right after the angles property, which will make mpu.int_angles a thing.

Code: Select all

 
    @property
    def int_angles(self) -> tuple:
        roll, pitch = self.angles
        return _A(roll//1, pitch//1)
Very nice, I could add that but its just as easy for me to do it - I guess i'm a control freak :D :D :D

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

Re: Beginner - help with self-balancing robot build

Tue Jun 15, 2021 10:27 pm

It's more efficient for you to do it, if you only have to do it once, as that will calculate angles, pack it in a tuple, unpack the tuple, and repack the tuple with floored values. My personal version is different but requires 3 changes. I didn't want to make it a whole thing just to get ints so, I posted that less efficient version to keep ot simple.

@I'm not learning assembly

Why not? It's just another language. You have to think a little different, but that's a good thing. I started with javascript, html, and css somewhere around the time it was invented, moved over to AS1, and then AS2. Concurrently, I dicked around with VBScript and .bat. Then I wandered over to PHP and threw in some SQL for good measure. I took a hard right into some QC while I also brushed up my skills in AS3. While programming in QC I wound up picking up some C++. At some point I wandered back over to html ,css and javascript when they gave it all a facelift, got bored and dove into HaXe hard, while still working on some stuff in AS3. Some jerk hacked my website so I learned Perl to make a miserable person really miserable. Somewhere in all of the above chain RegEx became a thing I do. I took a month off came back and put python under my belt, followed it up with micropython, with a tendency to convert my scripts to C and C++ in a manner congruent with USER_C_MODULES. There was plenty of wordpress, jquery, nodejs and PHPBB3 in all of that, as well. I left assembly out of all of that on purpose. When you realize that almost all of that is just the same thing with a different paint job and maybe a foreign part or 3, programming ceases to be a challenge. You need your RegEx, assembly and maybe even BrainFxxk just so it doesn't seem like every day it's scrambled eggs and toast for breakfast. Decades of scrambled eggs and toast for breakfast. At one point I was tempted to start programming in machine code just to make it as hard as it can possibly get.
"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

Wed Jun 16, 2021 7:48 am

OneMadGypsy wrote:
Tue Jun 15, 2021 10:27 pm
It's more efficient for you to do it, if you only have to do it once, as that will calculate angles, pack it in a tuple, unpack the tuple, and repack the tuple with floored values. My personal version is different but requires 3 changes. I didn't want to make it a whole thing just to get ints so, I posted that less efficient version to keep ot simple.
That makes sense, i'll keep it as is.

One question...

Why create the program as a class - in this instance, is it a personal style or does it bring a real benefit??

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

Re: Beginner - help with self-balancing robot build

Wed Jun 16, 2021 10:40 am

OK, so i have broader issues going on here than a slight difficulty reading code :)

It seems all along, my tests have may have been wrecked by interference. Today it seems worse for some reason but at least it prompted me to look deeper.

I get this error a lot...

Code: Select all

Traceback (most recent call last):
  File "<stdin>", line 78, in <module>
  File "<stdin>", line 52, in __init__
  File "mpu6050.py", line 225, in angles
  File "mpu6050.py", line 432, in __filtered_angles
  File "mpu6050.py", line 201, in data
  File "mpu6050.py", line 115, in __readBytes
OSError: [Errno 5] EIO
Which to me seems to be a total system crash?

The routine boils down to - connect USB, run the code, test the results on screen, seems stable at rest, connect 12v power to motors and it will start running when it should not, the numbers jump everywhere, it eventually hits top speed and stays there and almost immediately comes up with the crash. Only recovery is a soft reboot and start over.

Clearly I have electrical interference, I thought it might be magnetic but pulling the IMU away from the motors does nothing new. I thought it might be my PSU so tried the LiPo battery and same results on pure DC.

Currently looks like this...
Image

My thoughts are that it could be the long leads to the steppers, the close proximity of drives to pico to start with. So i might pull the drives off the breadboard and make a pcb to mount them on the lower deck - that means very short wires and away from the pico, its where they were going to go anyway.

Will try that for starters, failing that then I might need opto-isolators on the pico - drive lines but hope not.

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

Re: Beginner - help with self-balancing robot build

Wed Jun 16, 2021 2:34 pm

So we now have a much tidier setup, and the electrical interference seems 100% more stable now, no more crashes at least :D

It now looks like this...

Image


However the IMU is crazy sensitive and will happily just dance about with little or no input, if i stall the motors to remove vibrations, it will eventually stop, the slightest touch will have it off on a dance again, short video.....

https://davekearley.co.uk/wp-content/up ... 331440.mp4


Not sure if this is caused by not having PID in there, or something else, I have tried different mix settings on the complimentary filter, lower ramp settings, neither make any difference.

Back to the thinking stage and some reading I think, I cannot find any youtube history on this most likely because nobody posts their failures or development stages. The main factor I can see is that I have no PID and they all end up with it in the loop.....


Still, at least its electronically stable now :D :D :D

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

Re: Beginner - help with self-balancing robot build

Thu Jun 17, 2021 8:46 am

It seems everyone uses a loop time of 50ms or 20Hz and mine was running way faster than that so I added some simple measurements and a delay to give 20Hz but then realised the sample rate was the control factor for loop time so have increased that to give the same frquency, i'll try this out shortly. Not holding much hope but its another difference ironed out i think :D

Code: Select all

            if int(self.__mpu.angles[0]) in self.__pitch_range:                # Not fallen over yet
                st_time = utime.ticks_ms()
                pitch = self.__mpu.angles[0]                                   # Get pitch (using roll axis due to mount position)
                self.__Ldir.value(pitch >= 0)                                  # Set direction pins high or low based on pitch value
                self.__Rdir.value(pitch >= 0)                    
                pitch = int(abs(pitch))                                        # Get Integer and dump the sign
                self.__target_speed = int(_MAXSPEED/_MAXPITCH*pitch)           # Add 100 to the speed for every degree of pitch
                self.__speed_diff = self.__target_speed - self.__current_speed # Get the difference is between the target and current speed
                self.__current_speed += 0 if not self.__speed_diff else min(abs(self.__speed_diff), _RAMP) * (abs(self.__speed_diff)/self.__speed_diff) # See below
                self.__current_speed = int(self.__current_speed)               # Int only for final assignment to PWM
                if (self.__current_speed > 0):                                 # Cannot assign zero as a speed, zero = stopped
                    self.__Lpwm.duty_u16(_RUN)                                 # Enable PWM's
                    self.__Rpwm.duty_u16(_RUN)
                    self.__Lpwm.freq(self.__current_speed)                     # Assign speed to drives
                    self.__Rpwm.freq(self.__current_speed)
                    #utime.sleep_ms(21)   #delay to create 20Hz loop
                    en_time = utime.ticks_ms()
                    runtime = utime.ticks_diff(en_time, st_time)
                    print(runtime, self.__current_speed)
                    #print(self.__current_speed, self.__Ldir.value())  # lets see what goes on....
                else:
     

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

Re: Beginner - help with self-balancing robot build

Thu Jun 17, 2021 9:51 am

I can't figure out what it is doing at present,

My reckoning is that the best chance of maintaining balance its to act accordingly as soon as the tilt starts - it does not, but shows much hesitancy in direction - direction is based on angle so it indicates the IMU is still chucking garbage in and we all know garbage in = garbage out. Once the 'bot tilts one way or the other it must kick in the same direction only, its no good jerking backwards and forwards.

https://youtu.be/nNxNHaKN5w0

Is this indicative of not having PID? Maybe it is, PID would calculate a speed to try and get back to zero bubble but if its being fed bouncy numbers ?? My understanding is that if tuned, PID would take the reading, calculate a sufficient kick to reach target point (zero) and decelerate as it gets near to it to avoid overshoot, I think those are the P & I values?

I have no idea, simply because nobody has posted failures :( :( and none of the many builder i have asked seem willing to chime in which to me, at least in some way, indicates that they just copied a working build and have not much knowledge of what makes it tick under the hood :D

My guess is that PID would replace some of this code here...

Code: Select all

                self.__target_speed = int(_MAXSPEED/_MAXPITCH*pitch)           # Add 100 to the speed for every degree of pitch
                self.__speed_diff = self.__target_speed - self.__current_speed # Get the difference is between the target and current speed
                self.__current_speed += 0 if not self.__speed_diff else min(abs(self.__speed_diff), _RAMP) * (abs(self.__speed_diff)/self.__speed_diff) # See below
                self.__current_speed = int(self.__current_speed)               # Int only for final assignment to PWM

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

Re: Beginner - help with self-balancing robot build

Thu Jun 17, 2021 10:23 am

A small error in assumption, from reply #259 ?
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.
This only works when the figures are 3000 & 30 - I changed the _MAX_PITCH to 60 to allow for the numbers jumping about but the sum fails then
3000/30 = 100, 100 + 2deg = 102 :)
3000/60 = 50, 50 + 2deg = 52 :(

I'm not sure of the best thing here as if the numbers are jumping its all irrelevant, maybe pitch*100 or pitch+100 but just academic i think at present


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

Re: Beginner - help with self-balancing robot build

Thu Jun 17, 2021 2:49 pm

Well, i'm done, i have hacked the code about, even added what I was led to believe is a PID loop, no idea if it is correct but it does spit out numbers :D

I still get nothing but a gibbering wreck on the bench - its like the IMU is picking up every single vibration, multiplying that by 1000 and chucking it out as a serious tilt angle. I know its not a physically stable build but there are dozens built like this or worse and they all seem to function.

It's not a matter of PID tune as even at standstill the slightest vibration sends it off in a bi-directional quiver!

I have the IMU filter set for high gyro and low accelerometer as that is supposed to a stable setup but maybe prone to drift over time.

I'm seriously considering buying an Arduino, figuring out how to use it and stuffing that in just to prove it works but I really don't want to do that simply because once it works, i'll probably bin the Pico idea :( :( :(

Lost for ideas currently.


Current code....

Code: Select all

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

_MAXSPEED = const(3000)
_MINSPEED = const(20)
_MAXPITCH = const(30) # Any more than this means we have falen over
_RAMP     = const(100)
_RUN      = const(5000) # 32767=50% PWM duty cycle with 500us pulsewidth, 10000=15% & 150us pw, 5000=7.5% & 76us pw
_STOP     = const(0)
_L_DIRPIN = const(11)     
_L_DRIVE  = const(10)
_R_DIRPIN = const(13)  
_R_DRIVE  = const(12) 
_I2C_BUS  = const(1)   
_I2C_SDA  = const(14)  
_I2C_SCL  = const(15)

_P_SCALE = 40
_I_SCALE = 0.0
_D_SCALE = 0.0

Ldir = Pin(_L_DIRPIN, Pin.OUT, Pin.PULL_DOWN) 
Rdir = Pin(_R_DIRPIN, Pin.OUT, Pin.PULL_DOWN) 
Lpwm = PWM(Pin(_L_DRIVE, Pin.OUT))        
Rpwm = PWM(Pin(_R_DRIVE, Pin.OUT))        
currentSpeed = 0
pitchRange   = range(-_MAXPITCH, _MAXPITCH + 1)
pitch = 0
refPitch = 0  # This is the target angle for the robot when balancing
errorInteg = 0
runTime = 50 # Default time for 1st PID loop

#REMEMBER: If you change rate, dlpf and/or gyro, you have to calibrate with the new values first
cfg = dict(
    ofs         = (-2668, -4821, 1104, 100, 45, -34),# Offsets from robot
    #ofs         = (-674, 3071, 1368, 67, -17, 116),  # Offsets from dev board
    accel       = ACCEL_FS_2   ,                     # Set accelerometer range to 2g
    rate        = 54           ,                     # MPU6050_SPLRTDIV ~ comp filter samples at half of this number
    A           = 0.2          ,                     # Percent of accel data to use, 1-A is the percent of gyro to use
    dlpf        = DLPF_BW_42   ,                     # Set digital low pass filter to 42
    gyro        = GYRO_FS_250  ,                     # Set gyro range to 250deg/sec
    filtered    = FILTER_ANGLES,                     # Apply filtering to angles
    anglefilter = ANGLE_COMP   ,                     # Apply only complimentary filter to angles
)
mpu = MPU6050(_I2C_BUS, _I2C_SDA, _I2C_SCL, **cfg)


while True:
    if int(mpu.angles[0]) in pitchRange:                 # Check if we have fallen over
        startTime = utime.ticks_ms()                     # Get loop start time
        prevPitch = pitch                                # Store the pitch from the last cycle
        pitch = mpu.angles[0]                            # Get pitch (using roll axis due to mount position)
        Ldir.value(pitch >= 0)                           # Set direction pins high or low based on pitch value
        Rdir.value(pitch >= 0)                           # As above, other motor
        pitch = int(abs(pitch))                          # Get Integer and dump the sign      ??????????????
        error = pitch - refPitch                         # Calculate PID values
        errorDiff = (pitch - prevPitch) * 1000 / runTime # 50 is loop speed in ms, replaced by real loop speed when running
    
        if (error > 1.0):                                # Reset the integral if the robot passes the vertical
            errorInteg += error
        else:
            errorInteg = 0
    
        P = error * _P_SCALE                             # Calculate the PID control output
        I = errorInteg * _I_SCALE
        D = errorDiff * _D_SCALE
        currentSpeed = (int((P + I + D) + (P + I + D) * abs(P + I + D) / 1000)) 

        if (currentSpeed > _MAXSPEED):
            currentSpeed = _MAXSPEED
        elif (currentSpeed < -_MAXSPEED):
            currentSpeed = -_MAXSPEED
        
        if (currentSpeed > 0):                             # Cannot assign zero as a speed, zero = stopped
            Lpwm.duty_u16(_RUN)                            # Enable PWM
            Rpwm.duty_u16(_RUN)                            # Enable PWM
            Lpwm.freq(currentSpeed)                        # Assign speed to drive
            Rpwm.freq(currentSpeed)                        # Assign speed to drives
            endTime = utime.ticks_ms()                     # Get loop end time
            runTime = utime.ticks_diff(endTime, startTime) # Calculate loop speed
            print(runTime, currentSpeed, Ldir.value())#lets see what goes on....
        else:
            Lpwm.duty_u16(_STOP)                           # Stop moving, we are balancing
            Rpwm.duty_u16(_STOP)
            Lpwm.freq(_MINSPEED)
            Rpwm.freq(_MINSPEED)              
    else:
        Lpwm.duty_u16(_STOP)                               # Stop moving, we have fallen over again.
        Rpwm.duty_u16(_STOP)
        Lpwm.freq(_MINSPEED)
        Rpwm.freq(_MINSPEED)    
        

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

Re: Beginner - help with self-balancing robot build

Thu Jun 17, 2021 5:36 pm

davek0974 wrote:
Thu Jun 17, 2021 2:49 pm
I'm seriously considering buying an Arduino, figuring out how to use it and stuffing that in just to prove it works but I really don't want to do that simply because once it works, i'll probably bin the Pico idea :( :( :(
I guess a lot depends on what you want to do once it's self-balancing, what you hope to achieve from all this.

If it's a party piece or just to see something self-balance then an Arduino and already written code may well be the best course.

It may also be the best route if you do want to go further because there's nothing better than having something which works as a foundation to build upon, observe, analyse and tweak.

That may be the best way to see what is happening, understanding what's going on, and allow you to gain the knowledge needed for implementing it all in a Pico using MicroPython.

You have been at this for almost three weeks so one idea might be to take a break from it, let the frustrations subside, then come back to it. Then perhaps work through things; is the IMU returning the values it should, is the control loop correctly telling the motors what to do, are the motors doing what they are told, is the control loop correctly adjusting itself to what's going on. Somewhere in that you should be able to identify where the discrepancies are between what is happening and what should be happening.

The other option is to model it, though I have little idea how you would go about that. That may be easier than juggling a slippery eel when it comes to discovering those discrepancies. It may better allow 'if it's doing this, then it should be doing that, which causes this' to be worked through.

It may also be worth trawling the web, reading other people's project reports, getting to understand the maths and control principles better. Something is eventually going to click, what's not working will be obvious in hindsight. It's just the challenge of getting to that point. Unfortunately just bashing at things, banging one's head on the table, isn't the best way to get there.

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

Re: Beginner - help with self-balancing robot build

Thu Jun 17, 2021 6:01 pm

Hi

what i hope to achieve is to make one that works i guess, plus the challenge of doing it on the Pico and nobody else has yet - too new.

I really didn't want to learn Arduino, I struggle with uPython but its slowly starting to make a little sense now, Arduino code just looks ugly and awkward, I know the smarter guys just see code as code but not so for me, yet.

Lol, three weeks on here, and a few more before :) I'm retired so that's three weeks of probably 8 hours a day, reading stuff, trying stuff, a little making stuff but mostly looking at code and trying to figure out what the parts do, looking at other builds and discovering that they are all pretty much identical :D

Yes a break - i have three days away with the grandkids from tomorrow, :D :D

At present I don't think the IMU is giving what it should, or rather it is but i'm not sure that's what's needed. It seems extremely sensitive to vibration - at standstill it will behave erratically as shown in my videos and there is no way i can see it balancing with that type of input. Trouble is, despite weeks of searching I cannot find any data on what other builders are seeing from the IMU - they are pretty much all "just bung the IMU here, feed it into a PID loop, connect the motors here and hey presto here we go. There is no "well this didn't work" or "don't bother doing this" stuff i can find. The theory is clear but the actual data is hidden.

Looking at this video at present...
https://youtu.be/rJ3IljrzSXk

My data seems to be performing like the blue line on the graphs so maybe the filtering needs heavy tweaking, not sure, it would be good to visualise it like this though.

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

Re: Beginner - help with self-balancing robot build

Fri Jun 18, 2021 2:09 pm

I'm sorry the driver I made isn't working out for you. I haven't had any problems with it for my use, but my use doesn't involve rolling and balancing. I did the best I could with all the information that I could gather and implement. As far as the readings not being correct goes, that is not true. It is a very sensitive device and shaking it can produce wobbles in the readings (which is probably what PID is meant to buffer), but to claim they are wrong is incorrect. I've built most of a retro driving game with the device. The MPU pitch is used just like an accelerator and the roll is used like a steering wheel. If the values were wrong the game wouldn't be playable, because you have to get the perfect pitch and roll not to be pushed off the track or overdrive a corner in curves. The game plays very well. It's currently a little boring because all you do is drive by yourself around a track, but the steering and acceleration are proper.
"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

Fri Jun 18, 2021 7:06 pm

OneMadGypsy wrote:
Fri Jun 18, 2021 2:09 pm
I'm sorry the driver I made isn't working out for you. I haven't had any problems with it for my use, but my use doesn't involve rolling and balancing. I did the best I could with all the information that I could gather and implement. As far as the readings not being correct goes, that is not true. It is a very sensitive device and shaking it can produce wobbles in the readings (which is probably what PID is meant to buffer), but to claim they are wrong is incorrect. I've built most of a retro driving game with the device. The MPU pitch is used just like an accelerator and the roll is used like a steering wheel. If the values were wrong the game wouldn't be playable, because you have to get the perfect pitch and roll not to be pushed off the track or overdrive a corner in curves. The game plays very well. It's currently a little boring because all you do is drive by yourself around a track, but the steering and acceleration are proper.
You misunderstand or i worded it badly, my apologies. I do not think it's the driver at all: on its own, the numbers appear correct and stable, it does what is expected and is easy to use so it's clearly not the driver, once again my apologies.

It's either my usage of the numbers or a completely useless PID effort on my part, maybe even my choice of stepper motors or driver or something, I'm at a loss really at present. There are others that use the cheap geared DC motors and a full PWM drive - i might dump the steppers and try that idea.



And yes I know i was supposed to be away for few days but that idea was drowned out by torrential rain :cry: :cry:

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

Re: Beginner - help with self-balancing robot build

Sat Jun 19, 2021 1:48 am

I found this "all-in-one" arduino code for an SBR. PID is all the way at the bottom. Looking over the code, it has quaternion, euler and all of these other expectations that my driver does not provide. I think your problem is that my driver is simply not robust enough for your needs, and to make it robust enough will take way more work than I am willing to do, cause I'm not willing to do any (it's just true).


If you are willing to compromise your robot a bit here is an article about building a robot with the pico. It's not self balancing. It uses a caster for a third wheel. The up side is that you don't need an MPU-6050, at all. The down side is it's not what you are trying to build.

Here is another project for building a line follower robot. In a lot of ways it's just another iteration of the above robot, but it also follows lines.

This is not a tutorial, but could be used as inspiration for inventing your own robot style.

I posted these links here because you are trying to build a robot with a Pico, but the libraries for the type of robot you are trying to build don't seem to exist. However, there are still plenty of other robots you can build with the Pico. If you decided to build any of these other robots, you could still make use of the MPU-6050 as a tilt controller to drive the robot. I'm positive my driver can handle that, because, in a way, that's how I am successfully using 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

Sat Jun 19, 2021 5:47 am

Thanks OMG, appreciated.

I have seen many mentions of quaternions and euler angles but have also seen many builds that do not mention them or if the are used then the actual math is disguised as just numbers and not called quaternions etc.

The code below was the one from my early post way back in this thread, it seems to be very well copied and tweaked but this guy's name 'Joop Brokking' pops up everywhere, refuses to reply to youtube comment requests though ;)

Its Arduino of course, they all are, but fairly well commented, however, there is a number - "400" that pops up all through the code and I can't see why or what for yet. It also uses a fixed time loop - seems to be a missing point on the pIco as far as i can tell, even interrupt timers are not stable a i have proven with loop pulse timers.

Maybe the Pico will just not work for this?

A fair bit of the code is battery monitoring, I can dump that for now, I think it has steering, I can dump that as well, then there is a massive chunk that the Arduino uses to read I2C - the pico is far easier here so i can dump that. It reads the IMU registers direct but then converts into angles - it even states "the SBR is angle driven", well with your driver we have angles and good ones too. Basically pare it down to bare bones and see whats left.

There is PID, and a few other little oddities. Maybe I will sit down with a coffee tomorrow (busy today) and go through the guys videos and the code one more time, i do have a slightly better picture now, I'm getting good at using the PWM to generate pulses for the motors ( not actual PWM) I would call it "fixed pulse width modulation" myself :D but it works.

I'm not giving up just yet, but my mind is now more acceptable to the possibility that maybe, just maybe, the Pico cannot do this task. ;) ;)


Edit...(before i go out) :D

Unsure if its a thing yet but from watching his videos, https://youtu.be/VxpMWncBKZc he does explain a fair bit but only in Arduino terms of course, it seems he uses a fixed timer on a 20uS loop to generate one pulse for the stepper, this loop is fed by a count of desired pulses - the count is increased by the IMU & PID and decreased by the pulse loop iteration. As far as i can guess at present, i cannot send just one pulse as i am using a pulse stream (PWM). I can set that stream to one Hz but I'm not certain it's the same effect.

From watching many videos, at balance point, the SBR wheels barely move, well I cannot get that effect yet and I think that is the missing link. On some well-tuned builds it appears as completely static until pushed.

Working through his code, my MPU is setup with exactly the same parameters and even the calibration looks the same now thanks to OneMadGipsy's driver :)

Code: Select all

///////////////////////////////////////////////////////////////////////////////////////
//Terms of use
///////////////////////////////////////////////////////////////////////////////////////
//THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//THE SOFTWARE.
///////////////////////////////////////////////////////////////////////////////////////

#include <Wire.h>                                            //Include the Wire.h library so we can communicate with the gyro

int gyro_address = 0x68;                                     //MPU-6050 I2C address (0x68 or 0x69)
int acc_calibration_value = 1000;                            //Enter the accelerometer calibration value

//Various settings
float pid_p_gain = 15;                                       //Gain setting for the P-controller (15)
float pid_i_gain = 1.5;                                      //Gain setting for the I-controller (1.5)
float pid_d_gain = 30;                                       //Gain setting for the D-controller (30)
float turning_speed = 30;                                    //Turning speed (20)
float max_target_speed = 150;                                //Max target speed (100)

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Declaring global variables
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
byte start, received_byte, low_bat;

int left_motor, throttle_left_motor, throttle_counter_left_motor, throttle_left_motor_memory;
int right_motor, throttle_right_motor, throttle_counter_right_motor, throttle_right_motor_memory;
int battery_voltage;
int receive_counter;
int gyro_pitch_data_raw, gyro_yaw_data_raw, accelerometer_data_raw;

long gyro_yaw_calibration_value, gyro_pitch_calibration_value;

unsigned long loop_timer;

float angle_gyro, angle_acc, angle, self_balance_pid_setpoint;
float pid_error_temp, pid_i_mem, pid_setpoint, gyro_input, pid_output, pid_last_d_error;
float pid_output_left, pid_output_right;

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Setup basic functions
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void setup(){
  Serial.begin(9600);                                                       //Start the serial port at 9600 kbps
  Wire.begin();                                                             //Start the I2C bus as master
  TWBR = 12;                                                                //Set the I2C clock speed to 400kHz

  //To create a variable pulse for controlling the stepper motors a timer is created that will execute a piece of code (subroutine) every 20us
  //This subroutine is called TIMER2_COMPA_vect
  TCCR2A = 0;                                                               //Make sure that the TCCR2A register is set to zero
  TCCR2B = 0;                                                               //Make sure that the TCCR2A register is set to zero
  TIMSK2 |= (1 << OCIE2A);                                                  //Set the interupt enable bit OCIE2A in the TIMSK2 register
  TCCR2B |= (1 << CS21);                                                    //Set the CS21 bit in the TCCRB register to set the prescaler to 8
  OCR2A = 39;                                                               //The compare register is set to 39 => 20us / (1s / (16.000.000MHz / 8)) - 1
  TCCR2A |= (1 << WGM21);                                                   //Set counter 2 to CTC (clear timer on compare) mode
  
  //By default the MPU-6050 sleeps. So we have to wake it up.
  Wire.beginTransmission(gyro_address);                                     //Start communication with the address found during search.
  Wire.write(0x6B);                                                         //We want to write to the PWR_MGMT_1 register (6B hex)
  Wire.write(0x00);                                                         //Set the register bits as 00000000 to activate the gyro
  Wire.endTransmission();                                                   //End the transmission with the gyro.
  //Set the full scale of the gyro to +/- 250 degrees per second
  Wire.beginTransmission(gyro_address);                                     //Start communication with the address found during search.
  Wire.write(0x1B);                                                         //We want to write to the GYRO_CONFIG register (1B hex)
  Wire.write(0x00);                                                         //Set the register bits as 00000000 (250dps full scale)
  Wire.endTransmission();                                                   //End the transmission with the gyro
  //Set the full scale of the accelerometer to +/- 4g.
  Wire.beginTransmission(gyro_address);                                     //Start communication with the address found during search.
  Wire.write(0x1C);                                                         //We want to write to the ACCEL_CONFIG register (1A hex)
  Wire.write(0x08);                                                         //Set the register bits as 00001000 (+/- 4g full scale range)
  Wire.endTransmission();                                                   //End the transmission with the gyro
  //Set some filtering to improve the raw data.
  Wire.beginTransmission(gyro_address);                                     //Start communication with the address found during search
  Wire.write(0x1A);                                                         //We want to write to the CONFIG register (1A hex)
  Wire.write(0x03);                                                         //Set the register bits as 00000011 (Set Digital Low Pass Filter to ~43Hz)
  Wire.endTransmission();                                                   //End the transmission with the gyro 

  pinMode(2, OUTPUT);                                                       //Configure digital poort 2 as output
  pinMode(3, OUTPUT);                                                       //Configure digital poort 3 as output
  pinMode(4, OUTPUT);                                                       //Configure digital poort 4 as output
  pinMode(5, OUTPUT);                                                       //Configure digital poort 5 as output
  pinMode(13, OUTPUT);                                                      //Configure digital poort 6 as output

  for(receive_counter = 0; receive_counter < 500; receive_counter++){       //Create 500 loops
    if(receive_counter % 15 == 0)digitalWrite(13, !digitalRead(13));        //Change the state of the LED every 15 loops to make the LED blink fast
    Wire.beginTransmission(gyro_address);                                   //Start communication with the gyro
    Wire.write(0x43);                                                       //Start reading the Who_am_I register 75h
    Wire.endTransmission();                                                 //End the transmission
    Wire.requestFrom(gyro_address, 4);                                      //Request 2 bytes from the gyro
    gyro_yaw_calibration_value += Wire.read()<<8|Wire.read();               //Combine the two bytes to make one integer
    gyro_pitch_calibration_value += Wire.read()<<8|Wire.read();             //Combine the two bytes to make one integer
    delayMicroseconds(3700);                                                //Wait for 3700 microseconds to simulate the main program loop time
  }
  gyro_pitch_calibration_value /= 500;                                      //Divide the total value by 500 to get the avarage gyro offset
  gyro_yaw_calibration_value /= 500;                                        //Divide the total value by 500 to get the avarage gyro offset

  loop_timer = micros() + 4000;                                             //Set the loop_timer variable at the next end loop time

}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Main program loop
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void loop(){
  if(Serial.available()){                                                   //If there is serial data available
    received_byte = Serial.read();                                          //Load the received serial data in the received_byte variable
    receive_counter = 0;                                                    //Reset the receive_counter variable
  }
  if(receive_counter <= 25)receive_counter ++;                              //The received byte will be valid for 25 program loops (100 milliseconds)
  else received_byte = 0x00;                                                //After 100 milliseconds the received byte is deleted
  
  //Load the battery voltage to the battery_voltage variable.
  //85 is the voltage compensation for the diode.
  //Resistor voltage divider => (3.3k + 3.3k)/2.2k = 2.5
  //12.5V equals ~5V @ Analog 0.
  //12.5V equals 1023 analogRead(0).
  //1250 / 1023 = 1.222.
  //The variable battery_voltage holds 1050 if the battery voltage is 10.5V.
  battery_voltage = (analogRead(0) * 1.222) + 85;
  
  if(battery_voltage < 1050 && battery_voltage > 800){                      //If batteryvoltage is below 10.5V and higher than 8.0V
    digitalWrite(13, HIGH);                                                 //Turn on the led if battery voltage is to low
    low_bat = 1;                                                            //Set the low_bat variable to 1
  }

  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  //Angle calculations
  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  Wire.beginTransmission(gyro_address);                                     //Start communication with the gyro
  Wire.write(0x3F);                                                         //Start reading at register 3F
  Wire.endTransmission();                                                   //End the transmission
  Wire.requestFrom(gyro_address, 2);                                        //Request 2 bytes from the gyro
  accelerometer_data_raw = Wire.read()<<8|Wire.read();                      //Combine the two bytes to make one integer
  accelerometer_data_raw += acc_calibration_value;                          //Add the accelerometer calibration value
  if(accelerometer_data_raw > 8200)accelerometer_data_raw = 8200;           //Prevent division by zero by limiting the acc data to +/-8200;
  if(accelerometer_data_raw < -8200)accelerometer_data_raw = -8200;         //Prevent division by zero by limiting the acc data to +/-8200;

  angle_acc = asin((float)accelerometer_data_raw/8200.0)* 57.296;           //Calculate the current angle according to the accelerometer

  if(start == 0 && angle_acc > -0.5&& angle_acc < 0.5){                     //If the accelerometer angle is almost 0
    angle_gyro = angle_acc;                                                 //Load the accelerometer angle in the angle_gyro variable
    start = 1;                                                              //Set the start variable to start the PID controller
  }
  
  Wire.beginTransmission(gyro_address);                                     //Start communication with the gyro
  Wire.write(0x43);                                                         //Start reading at register 43
  Wire.endTransmission();                                                   //End the transmission
  Wire.requestFrom(gyro_address, 4);                                        //Request 4 bytes from the gyro
  gyro_yaw_data_raw = Wire.read()<<8|Wire.read();                           //Combine the two bytes to make one integer
  gyro_pitch_data_raw = Wire.read()<<8|Wire.read();                         //Combine the two bytes to make one integer
  
  gyro_pitch_data_raw -= gyro_pitch_calibration_value;                      //Add the gyro calibration value
  angle_gyro += gyro_pitch_data_raw * 0.000031;                             //Calculate the traveled during this loop angle and add this to the angle_gyro variable
  
  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  //MPU-6050 offset compensation
  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  //Not every gyro is mounted 100% level with the axis of the robot. This can be cause by misalignments during manufacturing of the breakout board. 
  //As a result the robot will not rotate at the exact same spot and start to make larger and larger circles.
  //To compensate for this behavior a VERY SMALL angle compensation is needed when the robot is rotating.
  //Try 0.0000003 or -0.0000003 first to see if there is any improvement.

  gyro_yaw_data_raw -= gyro_yaw_calibration_value;                          //Add the gyro calibration value
  //Uncomment the following line to make the compensation active
  //angle_gyro -= gyro_yaw_data_raw * 0.0000003;                            //Compensate the gyro offset when the robot is rotating

  angle_gyro = angle_gyro * 0.9996 + angle_acc * 0.0004;                    //Correct the drift of the gyro angle with the accelerometer angle

  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  //PID controller calculations
  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  //The balancing robot is angle driven. First the difference between the desired angel (setpoint) and actual angle (process value)
  //is calculated. The self_balance_pid_setpoint variable is automatically changed to make sure that the robot stays balanced all the time.
  //The (pid_setpoint - pid_output * 0.015) part functions as a brake function.
  pid_error_temp = angle_gyro - self_balance_pid_setpoint - pid_setpoint;
  if(pid_output > 10 || pid_output < -10)pid_error_temp += pid_output * 0.015 ;

  pid_i_mem += pid_i_gain * pid_error_temp;                                 //Calculate the I-controller value and add it to the pid_i_mem variable
  if(pid_i_mem > 400)pid_i_mem = 400;                                       //Limit the I-controller to the maximum controller output
  else if(pid_i_mem < -400)pid_i_mem = -400;
  //Calculate the PID output value
  pid_output = pid_p_gain * pid_error_temp + pid_i_mem + pid_d_gain * (pid_error_temp - pid_last_d_error);
  if(pid_output > 400)pid_output = 400;                                     //Limit the PI-controller to the maximum controller output
  else if(pid_output < -400)pid_output = -400;

  pid_last_d_error = pid_error_temp;                                        //Store the error for the next loop

  if(pid_output < 5 && pid_output > -5)pid_output = 0;                      //Create a dead-band to stop the motors when the robot is balanced

  if(angle_gyro > 30 || angle_gyro < -30 || start == 0 || low_bat == 1){    //If the robot tips over or the start variable is zero or the battery is empty
    pid_output = 0;                                                         //Set the PID controller output to 0 so the motors stop moving
    pid_i_mem = 0;                                                          //Reset the I-controller memory
    start = 0;                                                              //Set the start variable to 0
    self_balance_pid_setpoint = 0;                                          //Reset the self_balance_pid_setpoint variable
  }

  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  //Control calculations
  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  pid_output_left = pid_output;                                             //Copy the controller output to the pid_output_left variable for the left motor
  pid_output_right = pid_output;                                            //Copy the controller output to the pid_output_right variable for the right motor

  if(received_byte & B00000001){                                            //If the first bit of the receive byte is set change the left and right variable to turn the robot to the left
    pid_output_left += turning_speed;                                       //Increase the left motor speed
    pid_output_right -= turning_speed;                                      //Decrease the right motor speed
  }
  if(received_byte & B00000010){                                            //If the second bit of the receive byte is set change the left and right variable to turn the robot to the right
    pid_output_left -= turning_speed;                                       //Decrease the left motor speed
    pid_output_right += turning_speed;                                      //Increase the right motor speed
  }

  if(received_byte & B00000100){                                            //If the third bit of the receive byte is set change the left and right variable to turn the robot to the right
    if(pid_setpoint > -2.5)pid_setpoint -= 0.05;                            //Slowly change the setpoint angle so the robot starts leaning forewards
    if(pid_output > max_target_speed * -1)pid_setpoint -= 0.005;            //Slowly change the setpoint angle so the robot starts leaning forewards
  }
  if(received_byte & B00001000){                                            //If the forth bit of the receive byte is set change the left and right variable to turn the robot to the right
    if(pid_setpoint < 2.5)pid_setpoint += 0.05;                             //Slowly change the setpoint angle so the robot starts leaning backwards
    if(pid_output < max_target_speed)pid_setpoint += 0.005;                 //Slowly change the setpoint angle so the robot starts leaning backwards
  }   

  if(!(received_byte & B00001100)){                                         //Slowly reduce the setpoint to zero if no foreward or backward command is given
    if(pid_setpoint > 0.5)pid_setpoint -=0.05;                              //If the PID setpoint is larger then 0.5 reduce the setpoint with 0.05 every loop
    else if(pid_setpoint < -0.5)pid_setpoint +=0.05;                        //If the PID setpoint is smaller then -0.5 increase the setpoint with 0.05 every loop
    else pid_setpoint = 0;                                                  //If the PID setpoint is smaller then 0.5 or larger then -0.5 set the setpoint to 0
  }
  
  //The self balancing point is adjusted when there is not forward or backwards movement from the transmitter. This way the robot will always find it's balancing point
  if(pid_setpoint == 0){                                                    //If the setpoint is zero degrees
    if(pid_output < 0)self_balance_pid_setpoint += 0.0015;                  //Increase the self_balance_pid_setpoint if the robot is still moving forewards
    if(pid_output > 0)self_balance_pid_setpoint -= 0.0015;                  //Decrease the self_balance_pid_setpoint if the robot is still moving backwards
  }

  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  //Motor pulse calculations
  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  //To compensate for the non-linear behaviour of the stepper motors the folowing calculations are needed to get a linear speed behaviour.
  if(pid_output_left > 0)pid_output_left = 405 - (1/(pid_output_left + 9)) * 5500;
  else if(pid_output_left < 0)pid_output_left = -405 - (1/(pid_output_left - 9)) * 5500;

  if(pid_output_right > 0)pid_output_right = 405 - (1/(pid_output_right + 9)) * 5500;
  else if(pid_output_right < 0)pid_output_right = -405 - (1/(pid_output_right - 9)) * 5500;

  //Calculate the needed pulse time for the left and right stepper motor controllers
  if(pid_output_left > 0)left_motor = 400 - pid_output_left;
  else if(pid_output_left < 0)left_motor = -400 - pid_output_left;
  else left_motor = 0;

  if(pid_output_right > 0)right_motor = 400 - pid_output_right;
  else if(pid_output_right < 0)right_motor = -400 - pid_output_right;
  else right_motor = 0;

  //Copy the pulse time to the throttle variables so the interrupt subroutine can use them
  throttle_left_motor = left_motor;
  throttle_right_motor = right_motor;

  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  //Loop time timer
  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  //The angle calculations are tuned for a loop time of 4 milliseconds. To make sure every loop is exactly 4 milliseconds a wait loop
  //is created by setting the loop_timer variable to +4000 microseconds every loop.
  while(loop_timer > micros());
  loop_timer += 4000;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Interrupt routine  TIMER2_COMPA_vect
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ISR(TIMER2_COMPA_vect){
  //Left motor pulse calculations
  throttle_counter_left_motor ++;                                           //Increase the throttle_counter_left_motor variable by 1 every time this routine is executed
  if(throttle_counter_left_motor > throttle_left_motor_memory){             //If the number of loops is larger then the throttle_left_motor_memory variable
    throttle_counter_left_motor = 0;                                        //Reset the throttle_counter_left_motor variable
    throttle_left_motor_memory = throttle_left_motor;                       //Load the next throttle_left_motor variable
    if(throttle_left_motor_memory < 0){                                     //If the throttle_left_motor_memory is negative
      PORTD &= 0b11110111;                                                  //Set output 3 low to reverse the direction of the stepper controller
      throttle_left_motor_memory *= -1;                                     //Invert the throttle_left_motor_memory variable
    }
    else PORTD |= 0b00001000;                                               //Set output 3 high for a forward direction of the stepper motor
  }
  else if(throttle_counter_left_motor == 1)PORTD |= 0b00000100;             //Set output 2 high to create a pulse for the stepper controller
  else if(throttle_counter_left_motor == 2)PORTD &= 0b11111011;             //Set output 2 low because the pulse only has to last for 20us 
  
  //right motor pulse calculations
  throttle_counter_right_motor ++;                                          //Increase the throttle_counter_right_motor variable by 1 every time the routine is executed
  if(throttle_counter_right_motor > throttle_right_motor_memory){           //If the number of loops is larger then the throttle_right_motor_memory variable
    throttle_counter_right_motor = 0;                                       //Reset the throttle_counter_right_motor variable
    throttle_right_motor_memory = throttle_right_motor;                     //Load the next throttle_right_motor variable
    if(throttle_right_motor_memory < 0){                                    //If the throttle_right_motor_memory is negative
      PORTD |= 0b00100000;                                                  //Set output 5 low to reverse the direction of the stepper controller
      throttle_right_motor_memory *= -1;                                    //Invert the throttle_right_motor_memory variable
    }
    else PORTD &= 0b11011111;                                               //Set output 5 high for a forward direction of the stepper motor
  }
  else if(throttle_counter_right_motor == 1)PORTD |= 0b00010000;            //Set output 4 high to create a pulse for the stepper controller
  else if(throttle_counter_right_motor == 2)PORTD &= 0b11101111;            //Set output 4 low because the pulse only has to last for 20us
}

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

Re: Beginner - help with self-balancing robot build

Sat Jun 19, 2021 12:13 pm

The 400 is arbitrary, I believe. He's locking PID to a range of -400 to 400. I don't think this is a static number for building an SBR. I think it's a static number for his SBR. I could certainly be wrong, but the question I ask that gives me this impression is: What is a 400? When I was making the steering for my driving game, I was able to achieve OK steering by multiplying (road curve accumulated offset - steering turn accumulated offset) by speed * delta * 4. What is 4? 4 is nothing. It's just an arbitrary magnifier so the changes aren't so microscopic that it seems like nothing is happening. I believe his 400 is similar. It's just a range that keeps his thing behaving a certain way. Or more accurately, stops his thing from misbehaving a certain way. His real number is probably 402.49632679. Like I said. I could be wrong, but if so, what is 400? It's not an angle. It can be negative 400 so it isn't a frequency or a duty. It maybe could be a gyroscope reading, if so, why 400? Gyroscope readings are gone as quick as they come. What's special about 400, and why would you not want to know about anything above it?

Anyway, enough theory. If you really want to know what 400 is: rewrite his code for that part. You'll figure it out.
"Focus is a matter of deciding what things you're not going to do." ~ John Carmack

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

Re: Beginner - help with self-balancing robot build

Sat Jun 19, 2021 2:59 pm

davek0974 wrote:
Sat Jun 19, 2021 5:47 am

Maybe the Pico will just not work for this?

I find it very unfair to blame the PI PICO.
Technically, the PI PICO is worlds ahead of the Arduino!
But there are not yet 100,000 code examples that you can simply copy and then say you have built a robot.
I started learning MicroPython a few weeks ago, and I was helped very often here.
However, I spent all of these weeks learning and learning again!
There are many examples and tutorials about PI PICO and Micropython, but you have to invest your time.
If you're not ready for this, the Arduino really is the better way.

Here is a quick shot example:

viewtopic.php?f=146&t=314220

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

Re: Beginner - help with self-balancing robot build

Sun Jun 20, 2021 6:10 am

Nitro_fpv wrote:
Sat Jun 19, 2021 2:59 pm
davek0974 wrote:
Sat Jun 19, 2021 5:47 am

Maybe the Pico will just not work for this?

I find it very unfair to blame the PI PICO.
Technically, the PI PICO is worlds ahead of the Arduino!
But there are not yet 100,000 code examples that you can simply copy and then say you have built a robot.
I started learning MicroPython a few weeks ago, and I was helped very often here.
However, I spent all of these weeks learning and learning again!
There are many examples and tutorials about PI PICO and Micropython, but you have to invest your time.
If you're not ready for this, the Arduino really is the better way.

Here is a quick shot example:

viewtopic.php?f=146&t=314220
I wasn't blaming the Pico for what clearly are MY personal failures, its just that when solutions do not appear maybe i was trying to flog a dead horse as they say. Clearly you have proved me wrong 100% in your wonderful "quick-shot" build of what i try to achieve - this proves it with out a doubt that it can be done and can be done well and seemingly easily,

It may be I am trying to follow the wrong hobby here, obviously someone with no or very little code experience, poor grasp of maths, but good mechanical ability has little chance of stumbling upon the result he desires. You say you have been in it for a few weeks :) I have been at it for a few months and grasped maybe 10% of that needed :cry: I am getting a bit old now and have a few other issues though ;) so it was never going to be a walk in the park i think.

I will not bother going the Arduino route as it has already been done to death and just having a 'bot on the shelf was not my intention.

I will have a study of your code and see if i can find where I so miserably failed :D :D

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

Re: Beginner - help with self-balancing robot build

Sun Jun 20, 2021 6:12 am

OneMadGypsy wrote:
Sat Jun 19, 2021 12:13 pm
The 400 is arbitrary, I believe. He's locking PID to a range of -400 to 400. I don't think this is a static number for building an SBR. I think it's a static number for his SBR. I could certainly be wrong, but the question I ask that gives me this impression is: What is a 400? When I was making the steering for my driving game, I was able to achieve OK steering by multiplying (road curve accumulated offset - steering turn accumulated offset) by speed * delta * 4. What is 4? 4 is nothing. It's just an arbitrary magnifier so the changes aren't so microscopic that it seems like nothing is happening. I believe his 400 is similar. It's just a range that keeps his thing behaving a certain way. Or more accurately, stops his thing from misbehaving a certain way. His real number is probably 402.49632679. Like I said. I could be wrong, but if so, what is 400? It's not an angle. It can be negative 400 so it isn't a frequency or a duty. It maybe could be a gyroscope reading, if so, why 400? Gyroscope readings are gone as quick as they come. What's special about 400, and why would you not want to know about anything above it?

Anyway, enough theory. If you really want to know what 400 is: rewrite his code for that part. You'll figure it out.
Thanks OMG,

I see that now, he also did some wierd things to make the steppers behave in a linear fashion.

Thanks again for all your help and work over the last couple of weeks, its been great. :)

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

Re: Beginner - help with self-balancing robot build

Sun Jun 20, 2021 2:30 pm

Thanks again for all your help and work over the last couple of weeks, its been great.

You're welcome. I'll help you more if I can. Right now, though, I'm playing a hard game of catch-up. I just got an UDA1334 Audio DAC. I'm deep in the PIO on that. I wanted to take all of my drivers and crush them down into C without the ability to set pins, initialize them, and all of that (cause my device and it's connections are already determined). I wanted to make my driving game much better so I bought a better display (still a small ST7789, but a better one), and I rewrote a bunch of my graphics library to do a lot of work directly to addresses. I have a bit of hybrid solution going on. I don't need a 115200 bytes buffer anymore, and numerous things can be handled by passing off things directly to display addresses, but at the same time, some things are just faster if a precompiled buffer is blitted to the proper address, for those I implemented a run-length-encoding system so sprites can be stored with a small footprint and then expanded into the address buffer on each frame. For instance, the truck in my driving game is 12k, but run-length-encoded it is barely over 1k. Of course it needs to get blown up to 12k on every frame but it does this in about 2k chunks so, my memory usage is pretty good. The game is running smooth at 30fps, and there is a lot of floating point math going on so, I think the decisions I made are good ones. They seem to be anyway. I could maybe even get more than 30fps out of it, but the game is not complete so, changing render speed right now really isn't all that helpful. Really, 30fps for these types of games is plenty. NES rendered at 24fps IIRC so, I'm currently out-rendering the target graphical equivalent of my system. We'll see what happens when audio is included, though.

Anyway, all of that is just an example that I am hard at work, and I need to stay that way for a bit, but I haven't dropped out of giving you more help, if I can.
"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

Sun Jun 20, 2021 2:53 pm

OneMadGypsy wrote:
Sun Jun 20, 2021 2:30 pm

Anyway, all of that is just an example that I am hard at work, and I need to stay that way for a bit, but I haven't dropped out of giving you more help, if I can.

Sounds amazing, good luck and thanks for the continued offer. :D

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

Re: Beginner - help with self-balancing robot build

Mon Jun 21, 2021 10:57 am

Nitro_fpv wrote:
Sat Jun 19, 2021 2:59 pm
davek0974 wrote:
Sat Jun 19, 2021 5:47 am

Maybe the Pico will just not work for this?
Here is a quick shot example:

viewtopic.php?f=146&t=314220

Well, I have tried this code and couldn't get it to work - the result was almost identical to my own code.

This seems to be telling me i have other issues here - maybe the motors i chose are no good? The wheels seem fairly average size at 65mm dia.

I will admit i am trying the code with the MPU6050 simply because the price on the BNO055 is too high at present and delivery is long, but it's only using one axis and with the filtering we have should be ok (?) as it's pretty good at giving results in degrees now.

There isn't much left - its not the Pico, the drive is a common A4988, thats about all there is :cry: :cry: :cry:

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

Re: Beginner - help with self-balancing robot build

Mon Jun 21, 2021 1:45 pm

...maybe the motors I chose are no good
Simply hook up only the motors/driver with a potentiometer/rotary dial/slider ... something that you can emulate the MPU with manually. Then write a simple converter. For instance, let's assume you use a slider. The center position can be converted to equal zero degrees, and either side of center can be configured for positive/negative degrees accordingly. Then plug those degrees into whatever code is driving your motor. This way you create a system where the angles don't float and you can see how the motor behaves with you manually changing the degrees at whatever speed and amount you want. From there you can just keep adjusting your code til the motor responds the way you would expect it to from the changes you create.

If it's not obvious (maybe it's not), this is all bench work. The wheels won't be touching anything. No balancing or other behavior is being tested here. It's all about direct input and response. If you can get the kinks worked out this way, you simply replace the slider (or whatever you used) with the MPU and hopefully everything works or at the very least works much better. This is how I would do it ~ isolate the part entirely and make it work with manual emulation. Even if your motors or driver kind of suck with this manual emulation method you may still be able to write some code that works around whatever limitations/awkwardness the part has. It also should become more obvious what those limitations/awkwardness are. With the MPU it's just "out in the wild" and when the robot is running you don't know (I assume) what values are being created. With the manual approach you can say (for instance) "The slider is at about 10 degrees and the motor is freaking out" ... that's much better data to work with.

I wouldn't just give up on your motors/driver. I'll give you n example why. I have a card keyboard (literally a small card that is a keyboard). It works on 5v logic level. The Pico is 3.3v logic level. I wanted the card keyboard to work with the pico, without doing a bunch of crap with volt conversions. I ran some tests. At 3.3v all of the serial data was wrong ... but it was consistently wrong. For instance (this is a made up example) If I was expecting 0x63 it would always spit out 0x29 instead. I simply figured out what the new keymap was based on consistently wrong data. Sometimes you have to worry less about fighting something to give you the proper output based on the way you expect it to work, and worry more about getting the results you expect from the way it actually works, even if the way it works is wrong.
"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 21, 2021 2:03 pm

Thanks OMG,

I did have this before, just with two buttons, and the drivers/motors did seem to behave as expected - short clicks gave a couple of steps, direction could be hit as long as speed was fairly low (or the steppers cogged - that was the reason for looking at ramping) and so on.

I still the have the code somewhere in the junk folder, if i didn't morph it into something else that is :D

My trouble always seems to start when i introduce angular feedback - as seen in my video - it just does not sit still, plus the direction is always changing so it couldn't balance even if wanted to i think. https://youtu.be/nNxNHaKN5w0

It's very odd and yet I know now that thanks Nitro-FPV and his neat micro-coded SBR that it really can be easily done on the pico.

One difference is that he is using Euler-Yaw but I did ask and it seems its just angle data for one axis, no idea if this matters yet, however I can find very little evidence of others using or mentioning Euler angles so my concern is lessened here. Its really just roll around one axis thats needed, not full 3-d objects referenced to other objects.

Return to “MicroPython”