NXT-Segway with gyro and light sensors

A segway built with the Lego NXT framework using gyro sensors and a PID-controller:

The same PID-controller balancing the segway by measuring the reflected light intensity:

Exemplary, have a look at the source code for the gyro-driven model:

import nxt

bricks = list(nxt.locator.find_bricks())
brick = bricks[0].connect()

motors = [nxt.motor.Motor(brick, port) 
          for port in [nxt.PORT_A, nxt.PORT_C]]
gyro   = nxt.sensor.hitechnic.Gyro(brick, nxt.PORT_4)

def integrate(h = 1E-3, alpha = 0.05, PD = [30,15]):

    vel  = gyro.get_rotation_speed()  - offset

    # adams bashforth s = 1
    phi = Phi[-1] + h * (1.5 * vel  - 0.5 * V[-1])
    # exponential moving average estimating drift    
    s   =  alpha * phi + (1 - alpha) * S[-1]
    # compensate drift
    phi = phi - s

    # remember
    S.append(s)
    Phi.append(phi)
    V.append(vel)

    # PD-controller
    P, D  =  PD[0] * phi , PD[1] * vel
    power = min(127, abs(P + D)) * np.sign(P + D) 

    # run the motors
    [m.run(-power) for m in motors]

# all engines stop
[m.brake() for m in motors]

# play intro
for i in range(4):
    brick.play_tone(400 * 2**i, 1000)
    time.sleep(0.5)

# calibrate
gyro.calibrate()
offset = np.mean([gyro.get_rotation_speed() for i in range(5000)])   

Phi, V, S, B = [0], [0], [0], [0]

# integrate
for time in range(100000):
    integrate()

# all engines stop    
[m.brake() for m in motors]

blogroll

social