servo code

Bocephus · 2010-02-05 20:22

I got my first standard servo and have been looking over the datasheet and centerservo.spin file to understand how to move it correctly and hold the position. I had no problem converting to pasm but I would like some clarification. Am I correct in that the statement ctra[noparse][[/noparse] 8..0 ] := 0 is not required since the previous statement setting nco mode sets all other bits to zero? The servo is on p0. Also, why is frqa set to 1? Is it simply a non-zero value and could be any value?

'CenterServo.spin

CON
_clkmode = xtal1 + pll16x ' System clock &#8594; 80 MHz
_xinfreq = 5_000_000

PUB TestPwm | tc, tHa, t

  ctra[noparse][[/noparse] 30..26 ] := %00100 ' Configure Counter A to NCO
  ctra[noparse][[/noparse] 8..0 ] := 0
  frqa := 1
  dira[noparse][[/noparse] 0 ]~~
  
  ' Set up cycle and high times
  tC := (clkfreq/1_000_000) * 21_500
  tHa := (clkfreq/1_000_000) * 1500
  t := cnt ' Mark counter time
  
  repeat ' Repeat PWM signal
    phsa := -tHa ' Set up the pulse
    t += tC ' Calculate next cycle repeat
    waitcnt(t) ' Wait for next cycle

CON
  _clkmode = xtal1 + pll16x 
  _clkfreq = 80_000_000

PUB main
  cognew(@servo_test,0)

DAT
              org
servo_test    or        ctra,_nco
              mov       frqa,#1
              or        dira,counter            ' servo pin p0
              mov       time,cnt
              
:loop         mov       this_pulse,servo_center ' center servo
              call      #pulse_out
              mov       this_pulse,servo_left   ' left
              call      #pulse_out
              mov       this_pulse,servo_right  ' right
              call      #pulse_out
              jmp       #:loop
              
pulse_out     mov       counter,#32
[img]http://forums.parallax.com/images/smilies/tongue.gif[/img]_loop         neg       this_pulse,this_pulse
              mov       phsa,this_pulse
              add       time,delay
              waitcnt   time,delay
              djnz      counter,#[img]http://forums.parallax.com/images/smilies/tongue.gif[/img]_loop
pulse_out_ret ret             

_nco          long      %00100 << 26
counter       long      1                       ' used as servo pin, then counter
servo_left    long      80_000                  ' 1.0 ms
servo_center  long      120_000                 ' 1.5 ms
servo_right   long      160_000                 ' 2.0 ms
delay         long      1_600_000               ' 20  ms
this_pulse    res       1
time          res       1

Kye · 2010-02-06 05:20

So the ctra[noparse][[/noparse]8 .. 0] := 0 statment set the pin to output the servo pulse width to. Put your pin number in there where the 0 is.

The idea behind the driver is that the PHSA register is set to a negative number. So when you start adding 1 to the PHSA register starts counting down.

And all negative numbers have the MSB bit set which is the servo output. Have your cog reseting the counter in a loop and you get your servo pulse width output.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Nyamekye,

Bocephus · 2010-02-06 06:30

Thanks Kye. I knew the 8 bits were for the pin, but since the pin was p0 I didn't see the need in resetting them after setting nco mode. I guess I was just trying to verify that those 8 bits are cleared when setting nco.

Bocephus · 2010-02-06 18:37

For some reason I thought the sign was inverting each pass, I realize now it wasn't. So neg this_pulse,this_pulse should have simply been neg phsa,this_pulse and eliminate the latter mov. Both worked but I understand better now. Thanks again.

servo code

Comments