Smartpin mode for parallax feedback servo? — Parallax Forums

# Smartpin mode for parallax feedback servo?

Posts: 14,157

Want to use a smartpin to automate Capturing feedback signal from parallax feedback servo:

Feedback signal: PWM, 3.3V, 910 Hz, 2.7–97.1% duty cycle

Anybody want to suggest a smartpin mode for this?

Would be great if could just do rdpin and some math to get position…

• Posts: 15,420
edited 2024-05-12 00:16

You can go down two paths. Asynchronous measuring where pulse width and pulse period are integrated and you need read only one smartpin. Or use synchronous measuring where you precisely measure both the width and period in tandem but this requires two smartpins. Asynchronous is prone to some aliasing but is less resource hungry.

Asynchronous solution would use smartpin mode P_COUNT_HIGHS (%01111) with a sample period larger than two pulse periods. Can be much larger. Divide (use the cordic's QFRAC) it's Z width result by the sample period to find the width fraction.

Synchronous solution could use P_HIGH_TICKS (%10001) to find the width of a single pulse + P_EVENTS_TICKS (%10010) to find the period of that same pulse to the next. Again, divide width by period to get the fraction.

A more precise measurement can be made by combining both of the above. The natural clean precision of synchronous plus the enhancement of the longer sample period used for asynchronous. For this there is two sets of smartpin modes P_PERIODS_HIGHS (%10100) for accumulated widths of multiple pulses + P_PERIODS_TICKS (%10011) for accumulated periods of those pulses, or P_COUNTER_HIGHS (%10110) for widths of pulses + P_COUNTER_TICKS (%10101) for periods of pulses. Same dividing applies as above.

• Posts: 15,420

Of course, if period is not officially part of the signal then all you really want is the width. In which case P_HIGH_TICKS will precisely do the job all by itself.

• Posts: 8,999
edited 2024-05-12 07:31

Based on Evan's suggestions I did a bit of experimenting.

```pub main() | tCycle, tHigh, dc, angle

setup()
wait_for_terminal(true, 250)

term.fstr0(@"Parallax FB360 Servo Testing\r\r")

pinstart(SVO_FB, p_events_ticks, 1, 1)                        ' set measure period
waitms(2)
tCycle := rdpin(SVO_FB)

term.fstr2(@"%d    %.1fHz\r\r", tCycle, clkfreq*10 / tCycle)  ' show details

pinstart(SVO_FB, p_high_ticks, 0, 0)                          ' set to measure pulse

srvo.xwrite(1475)                                             ' move very slowly

repeat 40
waitms(250)
tHigh := rdpin(SVO_FB)
dc := 2_9 #> 100_0 * tHigh / tCycle <# 97_1
angle := ((dc - 2_9) * 360_0) / (97_1 - 2_9 + 1)
term.fstr2(@"%4.1f\% = %5.1f°\r", dc, angle)

srvo.xwrite(1500)

repeat
' done
```

Update: Corrected per Evan's comment below.

• Posts: 15,420
edited 2024-05-12 05:12

Ah, sorry, I didn't elaborate on smartpin setup. The correct Y setting for setup of period measure should be a 1 (A-input rise), not 2 (A-input edge).

I'm guessing the singular reading taken was when a very small pulse width was present, so thereby the measured fall-to-rise was almost as long as a full period.

There is quite a lot of duplication of function between the counter modes. Two of P_EVENTS_TICKS sub-modes are covered entirely in P_PERIODS_TICKS, and again in P_COUNTER_TICKS. All counting modes could have been packed into a wider selection of Y settings of a single counter mode. The four basic serial modes have spare X bits for combining them to one mode too. There might be opportunity for more config combinations with the serial.

• Posts: 8,999
edited 2024-05-12 07:32

Fixed. Thanks.

• Posts: 1,475
edited 2024-05-12 12:53

For those C lovers out there here is the same code:

```#include <stdio.h>
#include <stdlib.h>
#include <propeller.h>
#include <stdbool.h>
#include <memory.h>
#include "smartpins.h"
#include "servos.h"

#define SERVO 40
#define SVO_FB 41

char Buffer[128];

int main(int argc, char** argv)
{
int tCycle, tHigh;
float dc, angle;

gets(Buffer);

Servo_Enable(SERVO, 1500, 20000);

printf("Parallax FB360 Servo Testing\n");

_pinstart(SVO_FB, P_EVENTS_TICKS, 1, 1);      // set to measure period
_waitms(2);

tCycle = _rdpin(SVO_FB);

printf("%d --> %.1fHz\n", tCycle, (float)_clkfreq/ tCycle);

_pinstart(SVO_FB, P_HIGH_TICKS, 0, 0);        // set to measure pulse

Servo_Set(SERVO, 1475);                       // move servo slowly

while (true)
{
_waitms(250);
tHigh = _rdpin(SVO_FB);
dc = (float)tHigh / tCycle * 100;
if (dc < 2.9)
dc = 2.9;
if (dc > 97.1)
dc = 97.1;

angle = (dc - 2.9) * 360 / (97.1 - 2.9);  // from 360 manual
printf("%.1f%% --> %.1f \n", dc, angle);
}
}
```

The floating math does not come out the same though.

Mike

• Posts: 14,157
edited 2024-05-12 17:46

Thanks! Feedback seems to be working using @JonnyMac code.
But, servo itself isn't working. Have to check on that...
Ok, servo working. Guess code was set to 1475 us, which servo seems to ignore...

• Posts: 14,157

Guess now need some kind of PID loop if want to send servo to a particular angle...

• Posts: 8,999

I don't know what you're application is, but you might consider a robotic actuator like the LX-16 -- kind of a hobbyist priced Dynamixel (which are pricey).

After trying one I ended up bying the 5-unit kit which comes with a PC interface which I found useful for experimenting.

They're easy to use, run on a simple TTL network (one pin for many devices), handle decelerating into a position for you, and can be used in servo and motor modes. In servo mode, position data can be set and read back.

• Posts: 14,157

Wow, prices on those are great @JonnyMac
Last I looked into ones like that they were \$100 or so, as I recall...

• Posts: 8,999

The Dynamixel versions are very expensive. These have really good specs and the price is hard to beat. Now, with the FB360 you know where the device is through 360 degrees, though it takes a bit of work. If you don't need that much range, the LX-16A in servo mode will go 240 degrees and will return position data through a serial command.

If you're using them with a P2, you don't even need to invoke a serial cog if you've got some time in your process loop to deal with messages. I am nearly done with a simple object that uses inline PASM to send and [optionally] receive messages from the LX-16A. That was based on similar work I did with the Dynamixel for a lens focus and zoom controller.

Here's the inline code for talking to the LX-16A. Since the length of a response message is now known there is a 1.5 byte timeout.

```con

TX_UART = P_ASYNC_TX | P_OE                                   ' smart pin tx uart
RX_UART = P_ASYNC_RX                                          ' smart pin rx uart

pri lx_coms(pin, mode, p_tx, p_rx) : rxlen | bittix, x, txlen, tocount

'' Handle serial coms with LX bus servo
'' -- pin is serial io to lx servo
'' -- mode is M_TX (transmit only) or M_TXRX (transmit/recieve)
'' -- p_tx is pointer to transmit buffer
'' -- p_rx is pointer to recieve buffer (for mode M_TXRX)

bittix := baudticks                                           ' ticks/bit @ clkfreq

calc_checksum(p_tx, true)                                     ' add checksum to tx packet

org
cmp       mode, #M_TX           wcz
if_e            jmp       #tx_config
cmp       mode, #M_TXRX         wcz
if_e            jmp       #tx_config
jmp       #done                         ' exit if not a transmit request

tx_config               fltl      pin                           ' release pin
wrpin     #TX_UART, pin                 ' set to tx uart
mov       x, bittix                     ' setup baud
shl       x, #16
or        x, #(8-1)                     ' and bits
wxpin     x, pin
wypin     #0, pin
drvl      pin                           ' activate tx uart

rep       #1, #10                       ' idle for 1 byte period
waitx    bittix

transmit                mov       x, p_tx                       ' get data length
rdbyte    txlen, x
tjz       txlen, #done                  ' abort if no length in packet
add       txlen, #3                     ' set tx packet length (2x header + cs)

tx_loop                 rdbyte    x, p_tx                       ' get packet byte
wypin     x, pin                        ' load into sp uart
waitx     bittix                        ' let tx get started
rdpin     x, pin                wc      ' verify busy flag
if_c            jmp       #\$-1                          ' let byte finish
djnz      txlen, #tx_loop               ' if not end of packet, do next byte

cmp       mode, #M_TXRX         wcz     ' expecting response from this packet?
if_ne           jmp       #done

turn_around             fltl      pin                           ' clear tx configuration
wrpin     #RX_UART, pin                 ' setup for rx
drvl      pin
mov       rxlen, #0

mov       tocount, #60                  ' 60 bit periods = ~500us
.ta_loop                testp     pin                   wc      ' test for response
if_c            jmp       #get_byte
waitx     bittix
djnz      tocount, #.ta_loop
mov       rxlen, ##TIMEOUT              ' no packet, return timeout error
jmp       #done

rx_loop                 mov       tocount, #15                  ' set time-out counter
check_rx                testp     pin                   wc      ' anything waiting?
if_c            jmp       #get_byte                     ' yes, get byte from rx uart
waitx     bittix                        ' check for time-out
djnz      tocount, #check_rx
jmp       #done

get_byte                rdpin     x, pin                        ' read new byte
shr       x, #24                        ' align lsb
wrbyte    x, p_rx                       ' write to buffer
add       p_rx, #1                      ' bump pointer