Page 206 - Build Your Own Quadcopter_ Power Up Your Designs with the Parallax Elev-8

Chapter 7: Ser v o Motors and Extending the Ser v o Control System 185

waitcnt(clkfreq/2 + cnt)

repeat
repeat i from 0 to 2
pulse[i] := pulsewidth[i] ‘capture pulse values
from pins 14 to 16
waitcnt(clkfreq / 2 + cnt)

updateLCD(pulse[0],pulse[1],pulse[2]) ‘display pulse values
on LCD

out(i, pulse[i]) ‘send servo pulses out
2

if pulse[2] > 1600 ‘this is the start of the logic to
determine the LED lighting mode
led.start(0) ‘send a 0 to tell LED_Control to
enter mode0
if pulse[2] > 1200 AND pulse[2] < 1600
led.start(1) ‘send a 1 to tell LED_Control to
enter mode1
if pulse[2] < 1200
led.start(2) ‘send a 2 to tell LED_Control to
enter mode2

PRI updateLCD(value1, value2, value3) | numstr

numstr := num.dec(value1)
lcd.str(numstr)

lcd.str(string(“ “))

numstr := num.dec(value2)
lcd.str(numstr)

lcd.str(string(“ “))

numstr := num.dec(value3)
lcd.str(numstr)

lcd.str(string(13))

PUB out(_pin, _pulse)

servo.set(_pin, _pulse)

DAT