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?