Tracking servos?

M. K. Borri · 2007-05-21 00:55

I've started work on an enhanced autopilot thingy (let's call it navcom ai v2) and would like to know if anyone's already written the reverse of the servo pulse generator -- as in, something in assembly that takes in servo pulses on some pins and writes down in memory how long they were. If nobody's written one yet I will try to [noparse]:)[/noparse]

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
http://forums.parallax.com/showthread.php?p=650217

meow, i have my own topic now? (sorta)

Ariba · 2007-05-21 02:20

If you use a counter in pos-detector mode, you can measure a puls duration also in spin very exactly and with a resolution of 12.5ns @80 MHz ! And this with minimal effort.

ctra[noparse][[/noparse]30..26] := %01000      ' Set mode to "POS detector"
ctra[noparse][[/noparse]5..0] := PinNr              ' Set APIN
frqa := 1                            ' Increment phsa by 1

repeat
  phsa := 0                              'clear counter value
  waitpeq(PinMask, PinMask, 0)  'wait until puls start
  waitpne(PinMask, PinMask, 0)  'wait until puls end
  PulsDuration := psha              'read exact value from counter
  ...                                         'use PulsDuration

This is not a runable code, but shows you the way.

Andy

M. K. Borri · 2007-05-21 02:22

thanks! I can more or less do it in spin, but was wondering if someone had already done it in asm.... (I suck at asm still). Guess it's a good time as any to learn [noparse]:)[/noparse] also thanks again, I just had a loop, no counters, that's a lot better!!!!

In fact, I'm considering just reading the raw output from a receiver tuned to a R/C radio frequency (you know, bunch of servo-like pulses concatenated and separated by a longer pause), this way I can replace part of the RC radio altogether.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
http://forums.parallax.com/showthread.php?p=650217

meow, i have my own topic now? (sorta)

Post Edited (M. K. Borri) : 5/21/2007 2:26:54 AM GMT

AP · 2007-05-23 03:27

Here is what I have successfully been using for my "autopilot thingy" and it achieves 12.5 ns resolution at 80Mz clockfreq, measures 4 reciever channels at a refresh rate of 25 Hz, uses only 1 cog, and of course uses the built-in PHSA and PHSA timers in the cog. This is actual working code, although in my implementation it is one of several objects, and the "mother" object periodically accesses RAM locations of channels 1..4 and then either uses the data, or writes them in a log file on a micro SD card on-board the system.

The units for pulses are microseconds.

CON
· _clkmode = xtal1 + pll16x
· _xinfreq = 5_000_000
· rx1 = 1················ ' These are the I/O pins connected to R/C reciever channels r1..r4
· rx2 = 3
· rx3 = 5
· rx4 = 7

VAR
· long channels[noparse][[/noparse]4]
· long usec

PUB MAIN
' assumes and depends on pulse train order being 1-->2-->3-->4

> then eventually back to 1.
' rx1 through rx4 are Propeller I/O pins connected to R/C reciever, via 4.7k Ohm resistors and sharing common ground w/ Reciever.
' make sure pulse train from Rx is situated so that pins recieving signal is in order r1-->r2-->r3-->r4, otherwise
' sampling rate could drop as low as 12 Hz (not that that would necessarily be bad for certain data logging operations...
' GWS 'pico' 4 ch Rx has pulse sequence Rx channels 1-2-3-4. Other recievers may vary.

· usec := clkfreq / 1_000_000
· dira[noparse][[/noparse]rx1]~··························· ' set I/O to input
· dira[noparse][[/noparse]rx2]~··························· ' set I/O to input
· dira[noparse][[/noparse]rx3]~··························· ' set I/O to input
· dira[noparse][[/noparse]rx4]~··························· ' set I/O to input

· frqa := frqb := 1···················· ' enable the accumulations into PHSA and PHSB at 1 count unit per clock cycle

· repeat
··· repeat until ina[noparse][[/noparse]rx4] == 1········· ' wait for a high pulse to come by
··· repeat until ina[noparse][[/noparse]rx4] == 0········· ' and wait for I/O pin 3 to go low
··· CTRA := (%01000 << 26) + rx1
··· CTRB := (%01000 << 26) + rx2
··· PHSA := PHSB := 0
··· repeat until ina[noparse][[/noparse]rx2] == 1········· ' wait for a high pulse to start
··· repeat until ina[noparse][[/noparse]rx2] == 0········· ' and wait for I/O pin r2 pulse to finish
··· channels[noparse][[/noparse]0] := PHSA/usec··········· ' update ch_1 pulse length to RAM
··· channels[noparse][[/noparse]1] := PHSB/usec··········· ' update ch_2 pulse length to RAM

··· repeat until ina[noparse][[/noparse]rx1] == 1········· ' waits for channels 3 and 4 to pass once, otherwise about 800 clk cnts of ch3 get clipped
···
··· CTRA := (%01000 << 26) + rx3
··· CTRB := (%01000 << 26) + rx4
··· PHSA := PHSB := 0
··· repeat until ina[noparse][[/noparse]rx4] == 1········· ' wait for a high pulse to come by
··· repeat until ina[noparse][[/noparse]rx4] == 0········· ' and wait for I/O pin 3 to go low
··· channels[noparse][[/noparse]2] := PHSA/usec··········· ' update ch_3 pulse length to RAM
··· channels[noparse][[/noparse]3] := PHSB/usec··········· ' update ch_4 pulse length to RAM

Travis_ · 2009-01-11 02:50

Here's another way, using two functions pulled from the BS2_Function object (there was a bug in the original code, so I just pulled the relevant code out and fixed the bug).

This captures the incoming signal on P0 (in this case) and repeats it on P1. This allows monitoring, data logging and data manipulation.

I uploaded this example (Simple Radio Control Capture & Repeat to Servo) to the OBEX (obex.parallax.com/objects/401/).

CON

  _CLKMODE = XTAL1 + PLL16X
  _XINFREQ = 5_000_000

  'Pin on Prop Chip for LED Indicator
  PIN_LED = 15

  'Pins on Prop Chip for Servo Input/Output
  PIN_SERVO_IN = 0
  PIN_SERVO_OUT = 1

OBJ
  Debug  : "FullDuplexSerial"           'standard from propeller library
  SERVO  : "Servo32v3"                  'standard from propeller library
  
PUB Begin | x,y,z

  'start debugger
  debug.start(31,30,0,19200)
      
  'set pin direction for LED to output
  dira[noparse][[/noparse]PIN_LED]~~

  'flash the LED to confirm the program is running
  repeat 10
    !outa[noparse][[/noparse]PIN_LED]
    waitcnt(clkfreq / 2 + cnt) '0.5 seconds

  debug.str(string(16))
  debug.Str(string("Starting Simple RX Capture...", 13))

  'Start Servo handler
  SERVO.Set(PIN_SERVO_OUT,1422)
  SERVO.Start 
    
  repeat
    y := PULSIN(PIN_SERVO_IN,1)
    debug.str(string("BS2.PULSIN="))
    debug.Dec(y)
    debug.str(string(13))

    SERVO.Set(PIN_SERVO_OUT,(y * 2))
    debug.str(string("SERVO.Set(1,"))
    debug.Dec((y * 2))
    debug.str(string(")"))
    debug.str(string(13))

    debug.str(string(5,5))
    
    waitcnt(clkfreq / 4 + cnt) '0.25 seconds


PUB PULSIN (Pin, State) : Duration | us
{{ From BS2 Functions Library Object

   Reads duration of Pulse on pin defined for state, returns duration in 2uS resolution
   Shortest measureable pulse is around 20uS
   Note: Absence of pulse can cause cog lockup if watchdog is not used - See distributed example
    x := BS2.Pulsin(5,1)
    BS2.Debug_Dec(x)
}}
   us :=   clkfreq / 1_000_000                  ' Clock cycles for 1 us
    
   Duration := PULSIN_Clk(Pin, State) / us / 2 + 1         ' Use PulsinClk and calc for 2uS increments \

PUB PULSIN_Clk(Pin, State) : Duration 
{{ From BS2 Functions Library Object; modified waitpne and waitpeq functions to use |< Pin

   Reads duration of Pulse on pin defined for state, returns duration in 1/clkFreq increments - 12.5nS at 80MHz
   Note: Absence of pulse can cause cog lockup if watchdog is not used - See distributed example
    x := BS2.Pulsin_Clk(5,1)
    BS2.Debug_Dec(x)
}}

  DIRA[noparse][[/noparse]pin]~
  ctra := 0
  if state == 1
    ctra := (%11010 << 26 ) | (%001 << 23) | (0 << 9) | (PIN) ' set up counter, A level count
  else
    ctra := (%10101 << 26 ) | (%001 << 23) | (0 << 9) | (PIN) ' set up counter, !A level count
  frqa := 1
  waitpne(|< Pin, |< Pin, 0)                         ' Wait for opposite state ready
  phsa:=0                                                  ' Clear count
  waitpeq(|< Pin, |< Pin, 0)                         ' wait for pulse
  waitpne(|< Pin, |< Pin, 0)                         ' Wait for pulse to end
  Duration := phsa                                         ' Return duration as counts
  ctra :=0                                                 ' stop counter

Rob v.d. berg · 2009-03-04 09:32

Hello,

I use also the PULSIN_uS(Pin,State)·from the·BS2_Functions,·but sometimes in my application there is no puls (RC off), so the software is waiting!. Is·it simple to add·a timeout,·the maximum amount of time to spend in the PULSIN·PUB, if there is no·puls within the timeout, than exit PUB.

idea >> PULSIN_uS(Pin,State, Timeout)

Thanks for any idea, Rob

{{ code from Travis_ }}

··DIRA[noparse][[/noparse]pin]~
··ctra·:=·0
··if·state·==·1
····ctra·:=·(%11010·<<·26·)·|·(%001·<<·23)·|·(0·<<·9)·|·(PIN)·'·set·up·counter,·A·level·count
··else
····ctra·:=·(%10101·<<·26·)·|·(%001·<<·23)·|·(0·<<·9)·|·(PIN)·'·set·up·counter,·!A·level·count
··frqa·:=·1
··waitpne(|<·Pin,·|<·Pin,·0)·························'·Wait·for·opposite·state·ready
··phsa:=0··················································'·Clear·count
··waitpeq(|<·Pin,·|<·Pin,·0)·························'·wait·for·pulse
··waitpne(|<·Pin,·|<·Pin,·0)·························'·Wait·for·pulse·to·end
··Duration·:=·phsa·········································'·Return·duration·as·counts
··ctra·:=0

·

Tracking servos?

Comments