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

Chapter 5: Quadcopter Propulsors 119

waitcnt(clkfreq*5 + cnt) ‘delay (5 secs) to get
terminal program running
fdx.start(rxPin,txPin,0,baud) ‘start serial driver to PC
fdx.str(string(13,”Cluso’s Motor Control Test v002”,13))
fdx.str(string(“Press <space> to start”))
repeat
ch := fdx.rx
until ch := ”“
fdx.tx(13)
position := 80_000 ‘1ms (motor off)

cognew(@SingleServo,@position) ‘Start a new cog and run the
assembly code starting at the “SingleServo”
cell 0 and passing the address of the
“position” variable to that cog’s “par” reg
repeat
ch := fdx.rx
case ch
“0” : position := 80_000 ‘1ms OFF
“1” : position := 90_000 ‘1.125ms 12.5%
“2” : position := 100_000 ‘1.250ms 25%
“3” : position := 110_000 ‘1.375ms 37.5%
“4” : position := 120_000 ‘1.5ms 50%
“5” : position := 130_000 ‘1.625ms 62.5%
“6” : position := 140_000 ‘1.75ms 75%
“7” : position := 150_000 ‘1.875ms 87.5%
“8” : position := 160_000 ‘2ms ON

DAT
‘The assembly program below runs on a parallel cog and checks the
value of the “position” variable in the main hub RAM (which other
cogs can change at any time).
‘It then outputs a servo high pulse for the “position” number of
system clock ticks and sends a 20ms low part of the pulse.
‘It repeats this signal continuously and changes the width of the
high pulse as the “position” variable is changed by other cogs.

org ‘Assembles the next command to the first cell (cell 0) in
the new cog’s RAM
SingleServo mov dira,ServoPin ‘Set the direction of the
“ServoPin” to be an output (and
inputs)

Loop rdlong HighTime,par ‘Read the “position”
variable (at “par”) from main
RAM and store it as “HighTime”