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

Re: Beginner - help with self-balancing robot build

Wed Jun 23, 2021 5:37 pm

davek0974 wrote:
Wed Jun 23, 2021 3:26 pm
It seems I can get decent angles on the bench i think, but on the robot it's game over, for some reason the angles jump from +/-20 degrees - there is no way that can be stabilised, simply because those numbers are from a device that is not even moving :?
Unless that issue is resolved there isn't anything which is going to work.

As best as I can see, the possibilities for that are -

The I2C device being used is faulty
The I2C device is not powered correctly
The I2C device is not wired correctly
The I2C device has missing pull-ups or pull-downs
The I2C interface isn't right
Something is corrupting I2C transfers
The driver isn't interacting with I2C correctly
The driver is corrupting I2C data
The driver is incorrectly converting data
What's being shown as the result isn't what's actually being returned

That it appears to work on the bench but doesn't work on the robot suggest to me it's something about the robot causing the issue.

I would strip everything off the robot except the IMU. If it's stable on the bench but not when fitted to the robot it would seem it may be location related.
I would turn off all other equipment and the lights, try to nail down some specific small change which moves it from working to non-working.
davek0974 wrote:
Wed Jun 23, 2021 4:52 pm
So what was the solution - did you switch to circuitpython???
I don't believe Nitro_fpv had a problem in the first place, never needed a solution; it looks to me like it just worked - as would be anticipated.

Nitro_fpv's code describes using a MicroPython BNEO055 driver, and the "import machine" would confirm that it MicroPython and not CircuitPython.

Switching languages really won't help overcome a problem which isn't being caused by the choice of language.

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

Re: Beginner - help with self-balancing robot build

Wed Jun 23, 2021 5:39 pm

OneMadGypsy wrote:
Wed Jun 23, 2021 5:34 pm
Those are great numbers. That's probably what my actual numbers look like if I wasn't inting my floats. @ how do I get zero decimals. I don't use floats. I just drop the angles in int(pitch)

It could be that I have an MPU6050 lying around somewhere. :)
I'll let you know if I've found the part, then I'll test your driver on my Robi.
I just need an absolute angle, then we could include that in my Robicode.

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

Re: Beginner - help with self-balancing robot build

Wed Jun 23, 2021 5:39 pm

davek0974 wrote:
Wed Jun 23, 2021 5:32 pm
The numbers don't go crazy until the steppers are on - then its all hell breaks loose.
Tada !

EMI, probably.

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

Wed Jun 23, 2021 5:44 pm

Wait a minute. Your code: ofs = (-674, 3071, 1368, 67, -17, 116), # Offsets from dev board. Did you calibrate one board and then use those offsets for a different board? If so, why? That's not how it works. If it was you could just use my numbers which are completely different. If I'm right, maybe you'd like to calibrate the board you are actually using for the robot, so it can be calibrated and not spit out a 40 degree range of nonsense. Also, if hippy is right just make a little sock with alternating layers of plastic and aluminum foil (poor man's faraday cage), and slip the sock over the entire breadboard/PerfBoard/whatever you used. The layer the board should be inside of is a plastic one, alternate the layers out from that.
Last edited by OneMadGypsy on Wed Jun 23, 2021 5:50 pm, edited 2 times in total.
"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

Wed Jun 23, 2021 5:47 pm

davek0974 wrote:
Wed Jun 23, 2021 5:34 pm

This is totally baffling :? :? :? :?


As I wrote earlier, the IMU works with 4000 Perfect.
Do really use the pins on the PICO that I have in the code, especially the I2C pins!
And take the break out of the loop, that's bad, print still works.
Since you have the PICO on a breadboard, the easiest way is to take my pins, then you can try out my code directly and compare it with your code.

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

Re: Beginner - help with self-balancing robot build

Wed Jun 23, 2021 5:49 pm

I am fully agreeable that the issue with the MPU6050 could be EMI, i have tried some filtering but made no change, the only option i can think of to throw some opto isolators in there.

I have not tried the BNO055 on the robot as it will not even run on my coffee table setup.

I can look into complete galvanic isolation between the drives and pico/imu - will need to get some isolators and another battery maybe, the drives need the motor supply and a 3.3v logic supply - hopefully i can buck the 12v motor supply down to 3v3 for the drive logic.

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

Re: Beginner - help with self-balancing robot build

Wed Jun 23, 2021 5:51 pm

OneMadGypsy wrote:
Wed Jun 23, 2021 5:44 pm
Wait a minute. Your code: ofs = (-674, 3071, 1368, 67, -17, 116), # Offsets from dev board. Did you calibrate one board and then use those offsets for a different board? If so, why? That's not how it works. If it was you could just use my numbers which are completely different. If I'm right, maybe you'd like to calibrate the board you are actually using for the robot, so it can be calibrated and not spit out a 40 degree range of nonsense. Also, if hippy is right just make a little sock with alternating layers of plastic and aluminum foil (poor man's faraday cage), and slip the sock over the entire breadboard/PerfBoard/whatever you used.

No i used your little calibrate code to get those figures from the actual board i am working with - i just ran it again, fresh boot, this is the result

ofs=(-662, 3077, 1370, 69, -17, 115)


I will be looking at isolation tomorrow, thanks

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

Wed Jun 23, 2021 6:03 pm

the only option i can think of to throw some opto isolators in there.
Honest question: Why would that work? The MPU would still be exposed to the EMI, unless you think the EMI is only coming down the wire. I think that's optimistic. Maybe you're right, though.I wonder if you could just hang some multimeter leads around the motors, turn them on and see if it picks anything up. Maybe you could even use the continuity tester to see if it's so bad it will beep. :D I don't know if that's a true possibility, but it's funny to consider.
"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 23, 2021 7:40 pm

OneMadGypsy wrote:
Wed Jun 23, 2021 6:03 pm
the only option i can think of to throw some opto isolators in there.
Honest question: Why would that work? The MPU would still be exposed to the EMI, unless you think the EMI is only coming down the wire. I think that's optimistic. Maybe you're right, though.I wonder if you could just hang some multimeter leads around the motors, turn them on and see if it picks anything up. Maybe you could even use the continuity tester to see if it's so bad it will beep. :D I don't know if that's a true possibility, but it's funny to consider.

It is optimistic maybe but from past experience, interference is mostly via connection and not via emission/coupling. I will put the 'scope on it tomorrow and see if it shows me anything at all...

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

Re: Beginner - help with self-balancing robot build

Wed Jun 23, 2021 8:04 pm

Nitro_fpv wrote:
Wed Jun 23, 2021 5:47 pm
davek0974 wrote:
Wed Jun 23, 2021 5:34 pm

This is totally baffling :? :? :? :?


As I wrote earlier, the IMU works with 4000 Perfect.
Do really use the pins on the PICO that I have in the code, especially the I2C pins!
And take the break out of the loop, that's bad, print still works.
Since you have the PICO on a breadboard, the easiest way is to take my pins, then you can try out my code directly and compare it with your code.
Done, using your code 100% - i just added the "print" line to see results without motors etc..

Image

The code...

Code: Select all

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


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

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

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

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

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

And no , it does not run, it will just hang the pico - the only way to regain control is pull the plug it seems, even a soft reboot does nothing.

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

Wed Jun 23, 2021 8:49 pm

Did you make that little "pico playground"? It's really nice. I'm over here with 100 things squashed into a long breadboard. :D At least my wires are really neat (almost invisible). I run them under everything, even the Pico. Actually, some of my wires are overlapped just right so the MPU can't tilt in the breadboard holes. It's not pro, but this is still all in development so it doesn't really need to be. I still think your board is awesome, though. If you made that, excellent job! The only thing I would do to it (unless it's already there and I'm just not seeing it) is add a reset button.
Last edited by OneMadGypsy on Wed Jun 23, 2021 8:55 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

Wed Jun 23, 2021 8:53 pm

OneMadGypsy wrote:
Wed Jun 23, 2021 8:49 pm
Did you make that little "pico playground"? It's really nice. I'm over here with 100 things squashed into a long breadboard. :D At least my wires are really neat (almost invisible). I run them under everything, even the Pico. Actually, some of my wires are overlapped just right so the MPU can't tilt in the breadboard holes. It's not pro, but this is still all in development so it doesn't really need to be. I still think your board is awesome, though. If you made that, excellent job!

I wish i could claim i made it but no, i found it in a store called "The Pi Hut" it was ~£14 I think - its a very good little bit of kit, the robot has the usual long breadboard.

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

Wed Jun 23, 2021 8:59 pm

I'm going to buy one of those. That's a really nice little setup. It just needs a reset button though. Why does everybody miss that part? Maybe it does have one and I'm just not seeing it. If it doesn't it's still really nice, but I give it an A-, for missing the most obvious feature. I've been using my joke reset button ever since I made it. :D I like it cause I don't have to solder anything to the pico, and if I ever find some reason to need software reset I have that covered. It's a button plus an NPN transistor soldered on male headers that span from GND to RUN. It kinda looks like a muppet with a pompadour :D

Image
Last edited by OneMadGypsy on Wed Jun 23, 2021 9:04 pm, edited 1 time in total.
"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

Wed Jun 23, 2021 9:01 pm

Very strange.
I use 3 pieces of Adafruit BNO055 and all work with 4000 with the PICO Perfect.

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

Re: Beginner - help with self-balancing robot build

Wed Jun 23, 2021 9:09 pm

Nitro_fpv wrote:
Wed Jun 23, 2021 9:01 pm
Very strange.
I use 3 pieces of Adafruit BNO055 and all work with 4000 with the PICO Perfect.


Yep, its just my luck really :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

Wed Jun 23, 2021 9:14 pm

There's no such thing as luck. Skill and Odds. The more you can rely on skill the less you have to rely on odds.
"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 23, 2021 9:15 pm

OneMadGypsy wrote:
Wed Jun 23, 2021 9:14 pm
There's no such thing as luck. Skill and Odds. The more you can rely on skill the less you have to rely on odds.

Agree, at present i think i am playing the odds, and not winning, yet..... :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

Wed Jun 23, 2021 9:32 pm

.... and not winning, yet

I don't look at things like that, and you shouldn't either. What is winning? When you finally get something to stand up on 2 wheels? That's going to make you a winner? I know you probably think that I just sit down and write code and everything is unicorns and rainbows. Sometimes, that's true. Maybe even half of the time it's true. However! There is the other half of the time, where it's dragons and black holes. I'm still a winner. You know what makes me a winner? I don't give up, even if I really-really-really want to. When I was making this MPU driver there were 16 and 20 hour stretches where I was second guessing if I was capable of making anything worthwhile, at all, with this device, from a driver aspect. The only thing at one point that kept me going is I know that the only way to truly fail is to give up. I don't accept failure very well.

You're already a winner. You're a winner because no matter how much this thing beats you down you are determined to make your thing work. You've already won. Now you just have to prove it, and you will. After you get over this microscopic hump that has been there since the very beginning, it's going to be on like Donkey Kong. You'll have this thing doing wheelies, and that doesn't even make sense.
"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

Thu Jun 24, 2021 5:25 am

OneMadGypsy wrote:
Wed Jun 23, 2021 9:32 pm
.... and not winning, yet

I don't look at things like that, and you shouldn't either. What is winning? When you finally get something to stand up on 2 wheels? That's going to make you a winner? I know you probably think that I just sit down and write code and everything is unicorns and rainbows. Sometimes, that's true. Maybe even half of the time it's true. However! There is the other half of the time, where it's dragons and black holes. I'm still a winner. You know what makes me a winner? I don't give up, even if I really-really-really want to. When I was making this MPU driver there were 16 and 20 hour stretches where I was second guessing if I was capable of making anything worthwhile, at all, with this device, from a driver aspect. The only thing at one point that kept me going is I know that the only way to truly fail is to give up. I don't accept failure very well.

You're already a winner. You're a winner because no matter how much this thing beats you down you are determined to make your thing work. You've already won. Now you just have to prove it, and you will. After you get over this microscopic hump that has been there since the very beginning, it's going to be on like Donkey Kong. You'll have this thing doing wheelies, and that doesn't even make sense.

Its all just figures of speech, you are very good at reading people i think :D I too do not accept failure very well, and i'm certainly not going to be beaten by this thing, if I was, it would be in the bin already. I can fully understand your coding life, I have a past life of writing office based programs for my day job, invoicing, stock control, accounting etc, it was pretty much the same - some days easy and others dragged on for a few days of getting nowhere, especially when it came to V.A.T. and international payments etc. and this was at the same time as having a full day job in the same place as a manager :) Hard days but i miss them now. Why did I do two jobs?? It was a family business ;)

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

Re: Beginner - help with self-balancing robot build

Thu Jun 24, 2021 10:11 am

davek0974 wrote:
Wed Jun 23, 2021 8:04 pm
The code... And no , it does not run, it will just hang the pico - the only way to regain control is pull the plug it seems, even a soft reboot does nothing.
Start with code which only reads the IMU - Get that working before doing anything more complicated -

Code: Select all

from machine import Pin, I2C
from time    import sleep

sleep(1)

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

while True:
    print.format(imu.euler())
    sleep(1)
And double-check you have connected I2C to the correct pins. It's not easy to tell from the angle of the photo but it doesn't look right to me; looks like you are using pins 3 & 4 rather than 4 & 5.
OneMadGypsy wrote:
Wed Jun 23, 2021 8:59 pm
I'm going to buy one of those. That's a really nice little setup. It just needs a reset button though. Why does everybody miss that part? Maybe it does have one and I'm just not seeing it.
I would guess they did not think they needed one given there are four buttons, any of which can be linked to the reset pin. I would have added a dedicated reset button if I had been designing it.

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

Re: Beginner - help with self-balancing robot build

Thu Jun 24, 2021 10:17 am

hippy wrote:
Thu Jun 24, 2021 10:11 am
davek0974 wrote:
Wed Jun 23, 2021 8:04 pm
The code... And no , it does not run, it will just hang the pico - the only way to regain control is pull the plug it seems, even a soft reboot does nothing.
Start with code which only reads the IMU - Get that working before doing anything more complicated -

Code: Select all

from machine import Pin, I2C
from time    import sleep

sleep(1)

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

while True:
    print.format(imu.euler())
    sleep(1)
And double-check you have connected I2C to the correct pins. It's not easy to tell from the angle of the photo but it doesn't look right to me; looks like you are using pins 3 & 4 rather than 4 & 5.
Its the camera angle.

Using the code posted it fails, no error just does nothing and locks it all up..

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

Re: Beginner - help with self-balancing robot build

Thu Jun 24, 2021 10:29 am

davek0974 wrote:
Thu Jun 24, 2021 10:17 am
Using the code posted it fails, no error just does nothing and locks it all up..
Okay; try this and we can determine where it's hanging ...

Code: Select all

from machine import Pin, I2C
from time    import sleep

print("Starting")
sleep(1)
print("importing bno055")
from bno055 import *                            #IMU
print("imported bno055")
print("configuring I2C")
i2c = I2C(0, scl=Pin(5), sda=Pin(4), freq=4000) #IMU
print("configured I2C")
print("instantiating imu")
imu = BNO055(i2c)                               #IMU
print("instantiated imu")

while True:
    print("reading imu")
    results = imu.euler()
    print("got results")
    print(results)
    sleep(1)

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

Re: Beginner - help with self-balancing robot build

Thu Jun 24, 2021 12:12 pm

On the 'bot, things appear ok on the 'scope, power rails are clean, pwm is clean, I have added a little filter cap to 3v3 line and moved the Pico away from the drives, things seem more stable now. The I2C clock line looks clean but scoping the data line caused a crash. A short video below of it static, the changes in speed are the IMU picking up a slight wobble in the cradle, the actual figures being pumped out are also below as is the code.

If i can ask for step-mentoring at present i think it would help - i can plug numbers all day and still not hit the spot, i have no idea how easy or hard it should be, never having seen anything but a fully working setup. Maybe we can go one step at a time??? Maybe there is something staring me in the face that i cannot see?

Nitro_fpv, any chance you could do a short video of yours in a static position please? Are the wheels constantly active or dead when static?

Things i can see going on...
1 - When it leans more than a degree or two, the speed hits max, i know thats a PID effect, but it seems i start losing a lot of steps, the drives are set for max current now and the supply is 11.5v, maybe these drive boards are duff, they came from Amazon after all ;)

2 - These 'bots are supposed to be inverted pendulums. With the two motors opposite the battery i have it is not top-heavy, these motors are pretty heavy - is this a big issue, should i alter the build maybe?

3 - Theory has it that when it leans, it needs a good kick to get the base back under the top yes? This is what lead to my high P number, but it still does not get there, it just runs away at speed. Too little kick?

Static figures...

Code: Select all

-0.1768506 443
-0.1942803 484
-0.1877333 469
-0.1876881 468
-0.1862147 465
-0.185067 462
-0.1817361 454
-0.1723504 432
-0.1740366 436
-0.1851742 462
-0.1869423 467
-0.1926094 480
-0.2004439 499
-0.2217944 550
-0.2132569 530
-0.204184 508
-0.2151125 534
-0.2198808 545
-0.2203845 547
-0.2112998 525
-0.1918469 478

Code...

Code: Select all

#Motion & PID code adapted from Pascal Heubi "Nitro_fpv"
#MPU6050 Code and library from OneMadGypsy:- https://github.com/OneMadGypsy/upy-motion

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

_MAXSPEED = const(6000)
_MINSPEED = const(20)
_MAXPITCH = const(30)    # Any more than this means we have fallen over
_PWM_DUTY = const(10000) # 32767=50% PWM duty cycle with 500us pulsewidth, 10000=15% & 150us pw, 5000=7.5% & 76us pw
_RUN      = const(0)
_STOP     = const(1)
_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)
_D_ENPIN  = const(9)

Ldir = Pin(_L_DIRPIN, Pin.OUT, Pin.PULL_DOWN) 
Rdir = Pin(_R_DIRPIN, Pin.OUT, Pin.PULL_DOWN)
Dena = Pin(_D_ENPIN, Pin.OUT, Pin.PULL_DOWN)
Lpwm = PWM(Pin(_L_DRIVE, Pin.OUT))        
Rpwm = PWM(Pin(_R_DRIVE, Pin.OUT))
Lpwm.duty_u16(_PWM_DUTY)
Rpwm.duty_u16(_PWM_DUTY)       
Dena.value(_STOP)

utime.sleep(1) # Allow the MPU6050 to initialise

#REMEMBER: If you change rate, dlpf and/or gyro, you have to calibrate with the new values first
cfg = dict(
    ofs         = (-2796, -4805, 1090, 97, 40, -40),# Offsets from robot
    #ofs         = (-674, 3071, 1368, 67, -17, 116),  # Offsets from dev board
    accel       = ACCEL_FS_4   ,                     # Set accelerometer range to 4g
    rate        = 20           ,                     # MPU6050_SPLRTDIV ~ comp filter samples at half of this number
    A           = 0.011          ,                     # Percent of accel data to use, 1-A is the percent of gyro to use
    dlpf        = DLPF_BW_20   ,                     # 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)

# Interpolate the value from the PID controller for the PWM output
def interpolatePID(value, inMin, inMax, outMin, outMax):
    return max(min(outMax, (value - inMin) * (outMax - outMin) // (inMax - inMin) + outMin), outMin)

kP = 400             # PID P value
kI = 0              # PID I value
kD = 0              # PID D Value
previous = 0         # Variable value
integral = 0         # Variable value
calibration = 0      # Adjustment value for the center of gravity in deg

while True:
    pitch = mpu.angles[0]             # Get pitch (using roll axis due to mount position)    
    pitch = float(pitch)              # Convert pitch to float
    error = pitch + calibration       # Add calibration to measured pitch angle
    integral = integral + error       # Add error to PID integral value
    pidOutput = error * kP + integral * kI + (error - previous) * kD # PID calculation
    if pidOutput > 0:                 # Leaning forwards
        currentSpeed = (interpolatePID(pidOutput, 0, 1000, _MINSPEED, _MAXSPEED))  # Interpolate PID controller to PWM values
        Ldir.value(1)                 # Set direction pin high or low based on value
        Rdir.value(1)                 # As above, other motor
    else:                             # Leaning backwards
        currentSpeed = (interpolatePID(pidOutput, 0, -1000, _MINSPEED, _MAXSPEED)) # Interpolate PID controller to PWM values
        Ldir.value(0)                 # Set direction pin high or low based on value
        Rdir.value(0)                 # As above, other motor
    Dena.value(_RUN)                  # Enable drives
    if pitch > _MAXPITCH: Dena.value(_STOP)  # We have fallen over forwards Disable drives      
    if pitch < -_MAXPITCH: Dena.value(_STOP) # We have fallen over backwards Disable drives       
    currentSpeed = int(currentSpeed)  # Convert to int
    Lpwm.freq(currentSpeed)           # Set left motor speed
    Rpwm.freq(currentSpeed)           # Set right motor speed
    previous = error                  # Store error for PWM derivative value

    print(pitch, currentSpeed)


Video... https://youtu.be/3I0WHlRda60
Last edited by davek0974 on Thu Jun 24, 2021 12:40 pm, edited 1 time in total.

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

Re: Beginner - help with self-balancing robot build

Thu Jun 24, 2021 12:14 pm

hippy wrote:
Thu Jun 24, 2021 10:29 am
davek0974 wrote:
Thu Jun 24, 2021 10:17 am
Using the code posted it fails, no error just does nothing and locks it all up..
Okay; try this and we can determine where it's hanging ...

Code: Select all

from machine import Pin, I2C
from time    import sleep

print("Starting")
sleep(1)
print("importing bno055")
from bno055 import *                            #IMU
print("imported bno055")
print("configuring I2C")
i2c = I2C(0, scl=Pin(5), sda=Pin(4), freq=4000) #IMU
print("configured I2C")
print("instantiating imu")
imu = BNO055(i2c)                               #IMU
print("instantiated imu")

while True:
    print("reading imu")
    results = imu.euler()
    print("got results")
    print(results)
    sleep(1)

Here we go...

Code: Select all

>>> %Run -c $EDITOR_CONTENT
Starting
importing bno055
imported bno055
configuring I2C
configured I2C
instantiating imu

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

Re: Beginner - help with self-balancing robot build

Thu Jun 24, 2021 1:07 pm

davek0974 wrote:
Thu Jun 24, 2021 12:14 pm
Here we go...

Code: Select all

>>> %Run -c $EDITOR_CONTENT
Starting
importing bno055
imported bno055
configuring I2C
configured I2C
instantiating imu
So we need to know where it's hanging in the 'bno055' module.

Open "bno055.py" from your Pico file system. Completely replace the existing "def __init__" code ( around line 118) with the following -

Code: Select all

    def __init__(self, i2c, address=0x28, crystal=True, transpose=(0, 1, 2), sign=(0, 0, 0)):
        print("__init__[1]")
        self._argcheck(sign, 'Sign')
        print("__init__[2]")
        if [x for x in sign if x not in (0, 1)]:
            print("__init__[3]")
            raise ValueError('Sign values must be 0 or 1')
        print("__init__[4]")
        self.sign = sign
        print("__init__[5]")
        self._argcheck(transpose, 'Transpose')
        print("__init__[6]")
        if set(transpose) != {0, 1, 2}:
            print("__init__[7]")
            raise ValueError('Transpose indices must be unique and in range 0-2')
        print("__init__[8]")
        self.transpose = transpose
        print("__init__[9]")
        super().__init__(i2c, address, crystal)
        print("__init__[10]")
        self.buf6 = bytearray(6)
        print("__init__[11]")
        self.buf8 = bytearray(8)
        print("__init__[12]")
        self.w = 0
        print("__init__[13]")
        self.x = 0
        print("__init__[14]")
        self.y = 0
        print("__init__[15]")
        self.z = 0
        print("__init__[Exit]")
Save that edited "bno055.py" back to the Pico file system. Run the test code again.

My expectation is it will get to "__init__[9]" then hang, but we'll see.

Return to “MicroPython”