Self-balancing 42099 - help to make it work

[yuoppp]

Hello
I'm trying to implement the same thing as in this video: https://www.youtube.com/watch?v=qi6MRyLSrsI
I'm new to robotics algorithms so I need some help with the code.

I'm using the PID controller.

When I was writing the code, I was looking at similar implementation examples on arduino. I also found an article on pybrics called 'Gyro boy'.
ref1 - arduino example 1
ref2 - arduino example 2
ref3 - arduino example 3
ref3 - pybrics gyroboy

The gyroboy approach does not use PID controller, but I couldn't make it work on 42099.

My code works for about 1-4 seconds and the the vehicle starts to shake back and forth or just falls. I tried to reduce the kp value, but it causes another issue - the vehicle doesn't apply enough power to the motor so it falls.

What am I doing wrong? May be I'm missing something in the code or misunderstanding the PID algorithm?
Please, help me to make it work :)

[dlech]

Here is a snippet from the Gyro Boy program you linked:

        # calculate robot body angle and speed
        gyro_sensor_value = gyro_sensor.speed()
        gyro_offset *= (1 - GYRO_OFFSET_FACTOR)
        gyro_offset += GYRO_OFFSET_FACTOR * gyro_sensor_value
        robot_body_rate = gyro_sensor_value - gyro_offset
        robot_body_angle += robot_body_rate * average_control_loop_period

        # calculate wheel angle and speed
        left_motor_angle = left_motor.angle()
        right_motor_angle = right_motor.angle()
        previous_motor_sum = motor_position_sum
        motor_position_sum = left_motor_angle + right_motor_angle
        change = motor_position_sum - previous_motor_sum
        motor_position_change.insert(0, change)
        del motor_position_change[-1]
        wheel_angle += change - drive_speed * average_control_loop_period
        wheel_rate = sum(motor_position_change) / 4 / average_control_loop_period

        # This is the main control feedback calculation.
        output_power = (-0.01 * drive_speed) + (0.8 * robot_body_rate +
                                                15 * robot_body_angle +
                                                0.08 * wheel_rate +
                                                0.12 * wheel_angle)
        if output_power > 100:

It does some things different from your program.

1. Compare the robot_body_angle in that program to gyro_angle in yours. The change angle in each loop should be equal to the rate of change measured by the gryo sensor times the time since the last loop.
2. It uses 4 different measurements for feedback when calculating output_power, the angle of the robot body, the rate of rotation of the robot body, the angle of the wheels and the rate of rotation of the wheels. It looks like your program is just looking at the first two but doesn't take into account the movement of the wheels.
3. It only uses a proportional multiplier for each of these values. It seems to work well enough without the integral or derivative terms.

[laurensvalk]

We've got example code for balancers like those here: https://github.com/pybricks/pybricks-projects/blob/master/sets/mindstorms-robot-inventor/other-models/balancer/main.py

There's a demo here: https://www.youtube.com/watch?v=muVCJEmVa_M

If you can, I'd recommend building something small like these balancers first, so you can get a sense of how they work.

The truck is really heavy and tricky to get to work, especially if it's your first balancing robot.

[yuoppp]

I added the wheel rate and wheel angle and everything is ok now 👍
The truck balances very well. Thank you!

[minifig]

Do you mind sharing your code for this self balancing 42099?