Smartpin mode for parallax feedback servo?
Rayman
Posts: 14,632
in Propeller 2
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…
Comments
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.
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.
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 ' doneUpdate: Corrected per Evan's comment below.
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.
Fixed. Thanks.
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
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...
Guess now need some kind of PID loop if want to send servo to a particular angle...
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).
-- https://www.amazon.com/LX-16A-Bearing-Durable-Steering-17KG-CM/dp/B073WR3SK9
After trying one I ended up bying the 5-unit kit which comes with a PC interface which I found useful for experimenting.
-- https://www.amazon.com/LX-16A-Bearing-Durable-Steering-17KG-CM/dp/B073XY5NT1
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.
Wow, prices on those are great @JonnyMac
Last I looked into ones like that they were $100 or so, as I recall...
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 add x, #LX_DLEN 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 add p_tx, #1 ' advance pointer 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 add rxlen, #1 ' bump rx bytes received jmp #rx_loop ' wait for next byte done endI'm planning to use the LX-16A in a couple work projects so I made a P2-style accessory board for it. This lets me work with Dynamixels, LX-16s, and has two servo channels. I have the option to bring in external power so I don't put too much load on the 5v from the Eval board.