## Self balancing robot

Showroom for MicroPython related hardware projects.
Target audience: Users wanting to show off their project!
pythoncoder
Posts: 4645
Joined: Fri Jul 18, 2014 8:01 am
Location: UK
Contact:

### Re: Self balancing robot

How are you combining the gyro and accelerometer values? Many users recommend Kalman filters, but I found a complementary filter worked equally well and is simple and fast. My main loop is quite simple, but does have a couple of wrinkles. I maintain a constant loop period of 5ms. To achieve this on a Pyboard 1.0 I tweak the garbage collector to ensure it runs exactly once per iteration.

In my testing the main effort was in adjusting the PID constants so that the thing balanced when stationary and recovered well if I applied a brief push to the top. Then getting it to move or turn is just a matter of applying speed deltas. Hopefully the following is self explanatory. The PID constants are Kphi, Kdphi, and Kd

Code: Select all

``````def b():
gc.disable()
controlspeed = 0  # integrator output
fspeed = 0  # mean forward speed
fangle = 0.0  # Angle from complementary filter
csd = 0  # Differentiator output

stats = statgen() # TEST
led = pyb.LED(1)
while abs(fangle) < 45:  # Topple detect: 45 degs
start = ticks_us()
demandspeed, turnspeed = getdemand()  # Commands from radio
angle = imu.pitch()
rate = imu.gyro.x  # Rotates about x axis. Rate in deg/sec
fangle = compf(fangle, angle, rate, USLOOPTIME, 0.99)  # filtered angle
fspeed = (motor1.speed + motor2.speed)/2  # Forward speed
accel = fangle * Kphi + rate * Kdphi + (demandspeed - fspeed) * Kv
controlspeed += accel * MSLOOPTIME
motor1.speed = -controlspeed + turnspeed   # set motor speed -100 <= speed <= 100
motor2.speed = -controlspeed - turnspeed
gc.collect()  # Reduce GC delay from mS to ~26uS
dT = ticks_diff(ticks_us(), start)
if dT < USLOOPTIME:
sleep_us(USLOOPTIME - dT)  # Maintain constant loop time
else:
led.on()

motor1.speed = 0  # Crash! Stop and turn off motors
motor2.speed = 0
gc.enable()
``````
Peter Hinch