Pico: IMU absolute Euler angle and the PD controller

RP2040 based microcontroller boards running MicroPython.
Target audience: MicroPython users with an RP2040 boards.
This does not include conventional Linux-based Raspberry Pi boards.
Post Reply
Nitrosfpv
Posts: 16
Joined: Sat May 01, 2021 1:09 pm

Pico: IMU absolute Euler angle and the PD controller

Post by Nitrosfpv » Sat May 01, 2021 1:16 pm

I don't know how to solve this problem.
I have a floating platform that is stabilized in yaw.
So that I don't have any drift, I use the absolute Euler angle of an IMU in the PD controller.
The Euler angle goes from 0 ° to 360 °.
The problem, however, is the transition: when the platform is at around 0 °, the Euler always jumps between 0-360, then the PD controller goes crazy.
Here is a video link to the platform, still with a Helli Gyro that drifts in Yaw because there is no compass.

https://www.youtube.com/watch?v=WDvXwXwXYVo

And my test code:

Code: Select all

from machine import Pin, I2C, PWM
from time import sleep
sleep(1) #waiting for IMU
analogvalue = machine.ADC(28) #Poti RC
pwm = PWM(Pin(0)) #for the brushless controller
pwm.freq(200)

# Interpolates the value from the PD 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)

#Interpolates the analog potentiometer value for changing the euler_rc
def poti(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 *
i2c = I2C(0, scl=Pin(5), sda=Pin(4), freq=4000)
imu = BNO055(i2c)

KP = 40  #KP euler
KD = -10 #KD gyro
euler_rc = 0 #starting value

while True:
    gyro_imu = ('{2}'.format(*imu.gyro()))   #gyro yaw
    euler_imu = ('{0}'.format(*imu.euler())) #euler yaw
    euler_imu = float (euler_imu)
    gyro_imu = float (gyro_imu)
    poti_read = analogvalue.read_u16() #reading the potentiometer
    poti_out = (poti(poti_read, 200, 65535, -10, 10)) #Interpolates Poti - euler_rc
    euler_rc = euler_rc - poti_out #change the euler_rc start value with the potentiometer
    euler_diff = euler_imu - euler_rc #Difference for the PD controller
    balance_output = euler_diff * KP + gyro_imu * KD #PD Controller
    pwm_servo = (interpoliert(balance_output, -1000, 1000, 13080, 26190)) #Interpolates PD controller - PWM 
    pwm_servo = int (pwm_servo)
    pwm.duty_u16(pwm_servo) #for the brushless controller 
    #print(balance_output)
    #print(poti_out, euler_rc)

I only need the difference between setpoint and actual value Euler in the PD controller, even if the platform oscillates between approx. 2 ° and 358 °, for example.
How can I solve this problem?

hippy
Posts: 130
Joined: Sat Feb 20, 2021 2:46 pm
Location: UK

Re: Pico: IMU absolute Euler angle and the PD controller

Post by hippy » Sat May 01, 2021 4:37 pm

It's always worth including a link to where else an issue is being discussed, just to save people from going over ground which may have already been covered ... https://www.raspberrypi.org/forums/view ... p?t=310915

User avatar
pythoncoder
Posts: 5956
Joined: Fri Jul 18, 2014 8:01 am
Location: UK
Contact:

Re: Pico: IMU absolute Euler angle and the PD controller

Post by pythoncoder » Sat May 01, 2021 4:51 pm

If I understand your query it is about basic modular arithmetic. Consider these functions:

Code: Select all

def adiff(a, b):
    r = (a - b) % 360
    return r if r < 180 else r -360

def aadd(a, b):
    return (a + b) % 360
The aadd function adds a small offset to an angle, so that aadd(350, 3) produces 353°, and aadd(350, 13) produces 3°. The adiff function, which I think is what you are after, produces the difference between absolute angles. So adiff(350, 10) produces -20° and adiff(10, 350) produces 20°. There is a discontinuity when the difference reaches 180°: the sign changes because the shortest distance between the two angles means going round the circle in the opposite direction.
Peter Hinch
Index to my micropython libraries.

Nitrosfpv
Posts: 16
Joined: Sat May 01, 2021 1:09 pm

Re: Pico: IMU absolute Euler angle and the PD controller

Post by Nitrosfpv » Sat May 01, 2021 6:27 pm

hippy wrote:
Sat May 01, 2021 4:37 pm
It's always worth including a link to where else an issue is being discussed, just to save people from going over ground which may have already been covered ... https://www.raspberrypi.org/forums/view ... p?t=310915
You're absolutely right.
I will do this next time.

Nitrosfpv
Posts: 16
Joined: Sat May 01, 2021 1:09 pm

Re: Pico: IMU absolute Euler angle and the PD controller

Post by Nitrosfpv » Sat May 01, 2021 6:32 pm

pythoncoder wrote:
Sat May 01, 2021 4:51 pm
If I understand your query it is about basic modular arithmetic. Consider these functions:

Code: Select all

def adiff(a, b):
    r = (a - b) % 360
    return r if r < 180 else r -360

def aadd(a, b):
    return (a + b) % 360
The aadd function adds a small offset to an angle, so that aadd(350, 3) produces 353°, and aadd(350, 13) produces 3°. The adiff function, which I think is what you are after, produces the difference between absolute angles. So adiff(350, 10) produces -20° and adiff(10, 350) produces 20°. There is a discontinuity when the difference reaches 180°: the sign changes because the shortest distance between the two angles means going round the circle in the opposite direction.
exactly that at 180 ° is the problem.
I think I'll add up the angles.
Since the gondola will never make more than approx. 20 revolutions, this does not matter.
The other solution will probably be a rotation matrix, but that is going to be too complicated for me.

Nitrosfpv
Posts: 16
Joined: Sat May 01, 2021 1:09 pm

Re: Pico: IMU absolute Euler angle and the PD controller

Post by Nitrosfpv » Sat May 01, 2021 8:30 pm

I actually did it!
With the food for thought from hippy. :)
I have documented everything in the code.
I'm a little proud that I managed to do this.
I thank you all for your help.
My next questions will definitely come soon.

Code: Select all

from machine import Pin, I2C, PWM
from time import sleep
sleep(1) #waiting for IMU
analogvalue = machine.ADC(28) #Poti RC
pwm = PWM(Pin(0)) #for the brushless controller
pwm.freq(200)

# Interpolates the value from the PD 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)

#Interpolates the analog potentiometer value for changing the euler_rc
def poti(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)

analogvalue = machine.ADC(28) #Poti RC

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

last_euler = 0 #starting value 
korrektur = 0 #starting value
euler_rc = 0 #starting value

KP = 40  #KP euler
KD = -10 #KD gyro


while True:
    gyro_imu = ('{2}'.format(*imu.gyro()))   #gyro yaw
    euler_imu = ('{0}'.format(*imu.euler())) #euler yaw
    euler_imu = float (euler_imu)
    gyro_imu = float (gyro_imu)
    change = euler_imu - last_euler #Detecting a 360 ° jump
    if   change > +180 : korrektur = korrektur - 360 #If a jump forward, then - 360 ° for incremental
    elif change < -180 : korrektur = korrektur + 360 #If a jump backwards, then + 360 ° for incremental
    euler_incremental = euler_imu + korrektur # Count up and down
    last_euler = euler_imu #set new value
    poti_read = analogvalue.read_u16() #reading the potentiometer
    rc = (poti(poti_read, 200, 65535, -4, 4)) #Interpolates Poti
    if rc < +0.15 and rc > -0.15 : rc = 0 #dead zone potentiometer
    euler_rc = euler_rc + rc #Set target with the potentiometer
    euler_diff = euler_incremental + euler_rc #Difference value for the PD controller
    balance_output = euler_diff * KP + gyro_imu * KD #PD Controller
    pwm_servo = (interpoliert(balance_output, -1000, 1000, 13080, 26190)) #Interpolates PD controller - PWM 
    pwm_servo = int (pwm_servo)
    pwm.duty_u16(pwm_servo) #for the brushless controller
    #print(euler_incremental, euler_diff, euler_rc, rc)

Best regards

rafl
Posts: 10
Joined: Tue Mar 23, 2021 7:15 am

Re: Pico: IMU absolute Euler angle and the PD controller

Post by rafl » Sun May 02, 2021 9:34 pm

Nice job getting it all working, and thank you for sharing your solution! I learned a few things from it.

Another approach might be to represent your states as quaternions rather than euler angles. In that representation the math gets a bit easier and you won't need the same special-cases. It looks like the bno055 library you're using can already provide its data as quaternions.

User avatar
pythoncoder
Posts: 5956
Joined: Fri Jul 18, 2014 8:01 am
Location: UK
Contact:

Quaternions

Post by pythoncoder » Mon May 03, 2021 7:32 am

The BNO055 library does indeed offer quaternions and this doc shows how to use them with some Python classes. Despite having a fearsome reputation their use for 3D rotation is remarkably simple.
Peter Hinch
Index to my micropython libraries.

Nitrosfpv
Posts: 16
Joined: Sat May 01, 2021 1:09 pm

Re: Pico: IMU absolute Euler angle and the PD controller

Post by Nitrosfpv » Mon May 10, 2021 7:21 am

I have adjusted the control algorithm.
Now it does exactly what it should:
Accelerate up to a defined speed of rotation of the basket.
Decelerate before the target position.
Precision landing:

https://www.youtube.com/watch?v=9afIRWvspV8

See video :=)

Post Reply