Shop OBEX P1 Docs P2 Docs Learn Events
Smartpin mode for parallax feedback servo? — Parallax Forums

Smartpin mode for parallax feedback servo?

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

  • evanhevanh 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.

  • evanhevanh 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.

  • JonnyMacJonnyMac 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.

  • evanhevanh 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.

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

    Fixed. Thanks.

  • iseriesiseries 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

  • RaymanRayman 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...

  • RaymanRayman Posts: 14,157

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

  • JonnyMacJonnyMac 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).

    -- 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.

  • RaymanRayman 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...

  • JonnyMacJonnyMac 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
                            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
      end
    

    I'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.

Sign In or Register to comment.