## 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.
Nitrosfpv
Posts: 5
Joined: Sat May 01, 2021 1:09 pm

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

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: 34
Joined: Sat Feb 20, 2021 2:46 pm
Location: UK

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

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

pythoncoder
Posts: 5207
Joined: Fri Jul 18, 2014 8:01 am
Location: UK
Contact:

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

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

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

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

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: 5
Joined: Sat May 01, 2021 1:09 pm

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

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: 5
Joined: Sat May 01, 2021 1:09 pm

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

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

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.

pythoncoder
Posts: 5207
Joined: Fri Jul 18, 2014 8:01 am
Location: UK
Contact:

### Quaternions

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

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

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

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 :=)