Servo Control. Again?

SRLM · 2008-10-25 02:41

I might not be looking in the right place, but I haven't found any simple servo control routines. The Obex stuff has either assembly or a bunch of extra features (ramping, position feedback, etc.) and I couldn't find what I'm looking for on the forums or the PE labs, so I set out to modify some code that I found. Its from the TestDualPWM (Project 2).spin·file from the counters lab.

PUB TestPwm | tc, tHa, t, us         ' <- Add us     
  us := clkfreq/1_000_000                    ' <- Add
  ctra[noparse][[/noparse]30..26] := %00100                     ' Counters A and B &#8594; NCO single-ended
  ctra[noparse][[/noparse]8..0] := 13                           ' Set pins for counters to control
  frqa := 1                                  ' Add 1 to phs with each clock tick
                         
  dira[noparse][[/noparse]13] := 1                               ' Set I/O pins to output                    
  tC := 20_000 * us                          ' <- Change Set up cycle time
  tHa := 700 * us                            ' <- Change Set up high times 
  t := cnt                                   ' Mark current time.
  
  repeat tHa from (1200 * us) to (2200 * us)  ' <- Change Repeat PWM signal
    
    ' First pair of pulses
    ctra[noparse][[/noparse]8..0] := 13                          ' Set pins for counters to control
    phsa := -tHa                             ' Define and start the A pulse
    waitcnt(2200 * us + cnt)                 ' Wait for pulses to finish
    ' Wait for 20 ms cycle to complete before repeating loop
    t += tC                                  ' Calculate next cycle repeat
    waitcnt(t)                               ' Wait for next cycle

I took out my trusty Parallax oscilloscope, and found that to change the pulse width I need to change the first constant in the repeat loop. Thats the pulse width in us. What I'm wondering is, why? Also, what is tC, tHa, and t?

Finally, is there a simpler way without the loop? I think there must be, but what? PhiPi posted this snippet of code here, but it may be even more confusing than the previous.

 dira[noparse][[/noparse]EscPin]~~
  Time0 = clk
  repeat 50
    waitcnt(Time0 += clkfreq / 50)
    outa[noparse][[/noparse]EscPin]~~
    waitcnt(Time0 + Pulse * clkfreq / 1_000_000)
    outa[noparse][[/noparse]EscPin]~

Mike Green · 2008-10-25 03:05

Look in BoeBotBasic near the end. There's a simple servo driver "servoProcess" that's started in its own cog the first time it's used. It handles 3 servos and is about 17 lines in length.

SRLM · 2008-10-25 04:29

Good link. I got the function and cut it down some, but now it seems not to work:

PRI servoProcess | i, t, w, uS
   uS := clkfreq / 1_000_000
   outa[noparse][[/noparse]swingServoPin]~
   dira[noparse][[/noparse]swingServoPin]~~
   debug.str(string(13, "Begin pulse"))
   
   t := 0                                   ' Keep total of pulse widths
   repeat i from 0 to 2
                          
      w := swingServo * uS
      outa[noparse][[/noparse]swingServoPin]~~               ' If pulse width is non-zero
      waitcnt(w + cnt)                '  produce the requested pulse
      outa[noparse][[/noparse]swingServoPin]~
      t += w
 
      waitcnt(20_000*uS - t + cnt)             ' Pause for remainder of 20ms period

swingServoPin is a long that is set to 13 (my pin for servo). swingServo is the length (put to 750 for testing) of the pulse in ms. Yet my scope still shows a flat line, even though I know that it is running the function (courtesy of the debug). It can't get much simpler, yet here I am.

SRLM · 2008-10-25 04:31

Look at that. It was me all along: my scope wasn't waiting long enough, and so consistently missed the pulse. It works. Thanks again for the link.

Mike Green · 2008-10-25 04:37

Make sure you understand how a routine works before you modify it.

The repeat is there because the original routine was written for 3 servos. You're only using a single servo.

Remove the repeat (and the indenting that goes with it). Also remove the debug statement.

The servo pulse width is in microseconds. You should use 1500 for testing.

SRLM · 2008-10-25 05:26

For a while, I was thinking in BS2 terms so I was using 750 until it struck me that a BS2 is in units of 2 uS. :0

Anyway, here's the code that I think I'll end up with.

 'to test the servo functions
    repeat
      'debug.str(string("Begin pulsout loop"))
      timeTaken := cnt
      servoProcess(swingServoPin, swingServo)
      servoProcess(dropServoPin, dropServo)
      servoProcess(verticalServoPin, verticalServo)
      timeTaken := cnt - timeTaken
      'debug.str(string("Done pulsing, begin waiting"))
      
      waitcnt((20_000*uS) - timeTaken + cnt)
    
PRI servoProcess(pin, length) | w
   outa[noparse][[/noparse]pin]~
   dira[noparse][[/noparse]pin]~~
   'debug.str(string(13, "Begin pulse"))
   w := length * uS
   outa[noparse][[/noparse]pin]~~               ' If pulse width is non-zero
   waitcnt(w + cnt)          '  produce the requested pulse
   outa[noparse][[/noparse]pin]~

For the moment, I have it sending out pulses (defined earlier: 1700, 1500, 1500) to pins, then waiting for the remaining time to make it 20 ms between pulses. I also moved the uS definition to global. For my final program, I'll only repeat this loop two or three times.

Andy Lindsay (Parallax) · 2008-10-26 02:43

SRLM said...
...and I couldn't find what I'm looking for on the forums or the PE labs...

There's a Spin object that builds on the concepts in the Counters lab that can be used to control up to 14 servos with·one cog.· See the PE Kit Servo Control section under Applications in the Propeller Education Kit Labs thread.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Andy Lindsay

Education Department
Parallax, Inc.

SRLM · 2008-10-26 03:43

I saw that, but it's much more complex than I need, and since I'm just learning Spin (this is my first project), I decided to go with KISS.

Servo Control. Again?

Comments