A simple SPIN servo driver

Copper

So, I started fooling around with the propeller just a couple months ago. It's not only my first micro-controller, but also my first real programming experience---excluding the lego mindstorms stuff. It's been challenging, but incredibly enlightening and mostly quite enjoyable.

Currently I'm working on a simple SPIN servo driver (loosely based on the 32servo driver by Beau Schwabe). Unfortunately I've hit a small road block.

After adding my 'Ramp Servo block of code, the 'begin sending signal repeat loop stopped repeating.

{{Basic Servo Stuff}}CON


  _clkmode = xtal1 + pll16x                  
  _xinfreq = 5_000_000                      


  Pulse_Inc = 620
  #0, For4, For3, For2, For1, stop, Rev1, Rev2, Rev3, Rev4                               


OBJ


  pst : "Parallax Serial Terminal"


PUB Main


  Run_Servo(12, For3, 1000)
  Run_Servo(12, For1, 1000)
  Run_Servo(12, Rev4, 1000)
  Run_Servo(12, For3, 1000)
    
PUB Run_Servo(servoPin, speed, duration) | Pulse_Res, Pulse_Delay, Pulse_Width, time, Duration_Res, run_time, i 


  pst.Start(57600)
  waitcnt(clkfreq+cnt)


  'configure counterA
  ctra := (100 << 26 | servoPin)          'Counter A to NCO & Apin to servoPin 
  frqa := 1                                  'increment phsa by 1
  dira[servoPin]~~                           'set servoPin to output


  'configure pwm signal
  Pulse_Res := clkfreq/1_000_000             'set Pulse_Res == 1 microsecond  Pulse_Res == PulseWidth Resolution
  Pulse_Delay := Pulse_Res * 20_000          'determine pulse delay
  Pulse_Width := Pulse_Res * Speeds[speed]   'determine pulse width 
  time := cnt                                'mark counter time


  'report PWM config values
  pst.str(string(13, "****Config' PWM****"))
  pst.str(string(13, "Pulse_Res==", 13))
  pst.bin(Pulse_Res, 32)
  pst.str(string(13, "Pulse_Delay==", 13))
  pst.bin(Pulse_Delay, 32)
  pst.str(string(13, "Pulse_Width==", 13))
  pst.bin(Pulse_Width, 32)
  
  'detirmine runtime
  Duration_Res := clkfreq/1000               'set Duration_Res == 1millisecond   Duration_Res == Duration Resolution
  run_time:=((duration*Duration_Res)+time)   'set run_time


  'report runtime calculation values
  pst.str(string(13, 13, 13, "****Determine Runtime****"))
  pst.str(string(13, "Duration_Res==", 13))
  pst.bin(Duration_Res, 32)
  pst.str(string(13, "run_time==", 13))
  pst.bin(run_time, 32)
  pst.str(string(13))
  
  i:=0
     
  'begin sending signal
  repeat until time==run_time                'start PWM signal
    i++
    pst.str(string(13, 13, "****Pulse#"))
    pst.dec(i)
    pst.str(string("****"))
    
    'Ramp servo
    if Pulse_Width > PULSE[servoPin]        
      PULSE[servoPin]+=Pulse_Inc
      pst.str(string(13, "Ramp UP"))
    if Pulse_Width < PULSE[servoPin]
      PULSE[servoPin]-=Pulse_Inc
      pst.str(string(13, "Ramp Down"))
                                                       
    pst.str(string(13, "PULSE=="))
    pst.bin(PULSE, 32)
        
    phsa := -PULSE[servoPin]                 'send pulse for Pulse_Width seconds 
    time += Pulse_Delay                      'calculate end of cycle 
    waitcnt(time)                            'wait for next cycle


DAT
             'For4, For3, For2, For1, stop, Rev1, Rev2, Rev3, Rev4
  Speeds long 1000, 1200, 1450, 1500, 1515, 1530, 1580, 1830, 2000    
  PULSE long 0[32]

I tried replacing my "if statements" with an "if-else statement", and a "case statement". Then I tried writing all three approaches into their own methods but got the same results. All attempts seem to fail after executing the "condition check"

My serial terminal output looks like:

if statement test above

case statement test below

I'm not sure why my "case statement" doesn't at least report Ramp UP or Ramp Down.

Or why the "if statement" wouldn't work.

If my repeat loop was just too busy, and thus sending signals at the wrong times, I would still expect to see multiple loops in the serial terminal output.

PS: Before adding the ramping code, and all the serial terminal stuff, it worked exactly as expected.
Also, of course I could use Beau Schwabe's driver, but more than I need a functional servo driver, I'm looking to learn more about servos and programming

Thanks for your time

JonnyMac

If one takes advantage of the counters to generate the servo pulse(s), there is a lot of time left (2.5ms) to do ramping calculations. I've never needed 32 servos and I didn't want to use an entire cog to add ramping so I wrote this object.

-- http://obex.parallax.com/objects/445/

It will control up to 8 servos (by using the second counter you could go to 16) and it does these things:

* one servo pulse at a time
* leading-edge to leading-edge on all channels is 20ms
* ramping is understandable: it is in microseconds (servo movement) per second of real time
* 100% Spin -- no assembly required

You may find some useful code in the object. For features I was trying to mimic the SSC-32 which is a very popular servo controller.

On what you're doing: Even though the serial driver is in its own cog, putting debug statements in your object method is consuming time that may be affecting performance. You have a lot of local variables so you may want to write the address of the first to a global pointer that can be used by another cog for transmitting the values. This is a little advanced, but well worth learning how to do.

Update: I started a thread on how to spy on your Spin cogs without affecting their performance through the insertion of serial statements:
-- http://forums.parallax.com/showthread.php?142041-For-Newcomers-Spying-on-Your-Own-Code-(without-special-tools)

Copper

I'm going to have to spend some more time looking over JonnyMac's code, and I did wonder if my loop had gotten so long as to be sending pulses at the wrong time; but I do not understand why my driver would simply stop after the first loop. I would have expected it to continue sending pulses for the intended duration, merely at the wrong times. Is there a simple explanation for this behavior that will help me avoid similar situations in the future?

PS: The driver stops after the first loop with and without the serial terminal commands. To me, It really appears to be something with my 'ramp servo's condition check (the If/case statement if "condition check" is a made up term).

kuroneko

First, don't start the serial terminal inside the method you're calling 4 times (it gets in its own way). The stop condition you're experiencing is waitcnt wrap meaning your initial time calculations simply take to long. IOW the first call to waitcnt in the loop just misses and has to wait for a whole period (~53.68sec @80MHz). You should therefore move time := cnt closer to the runtime calculation (skip the first debug block, it's advisable to skip the second one too). This at least will get you a more lively loop. Then you can deal with your logic.

Also, the last pst.bin should probably output PULSE[servoPin] rather than just PULSE[0].