Shop OBEX P1 Docs P2 Docs Learn Events
Converting Brads to PSC 2us Steps and Back Again — Parallax Forums

Converting Brads to PSC 2us Steps and Back Again

ZootZoot Posts: 2,227
edited 2006-09-14 04:38 in BASIC Stamp
I'm working on a program where I store servo positions for my pan/tilt/sonar units as BRADS in single bytes in SPRAM. I also read the actual positions from a Parallax Servo Controller and convert the positions back to BRADS for storage and parsing.

128 - 64 - 0 brads corresponds to 180 - 90 - 0 degrees, and, on most of my servos, roughly 1200 - 750 - 300 in 2us steps for the PSC.

All my code works perfectly, except that my conversions back and forth don't always match. They match most of the time, but sometimes are off by 1 brad. Since part of my engine checks that the sent position (in brads) and the read position (converted to brads) match, it's a problem.

Here's the relevant code where I convert my desire position in BRADS to the equiv in PSC 2us steps (offset and calibrated for my servos). I've omitted a lot of it to make it easier to look at, but I can post more if anyone is curious. Like I said, the code all works perfectly, just my math on the conversion of the read position is sometimes off by a unit.

'parameters: PSC ServoChan

Servo_Send:
   GOSUB Get_ServoInfo    'fetches SPaddr of servo data, desired position in brads, last sent position in brads, min and max limits
                                        'and the word servopsc -- a scaler where x many 2us PSC steps = ~64 brads on that servo         
   servo = ( servo + 1000 MAX ( servo(mx) + 1000 ) MIN ( servo(mn) + 1000 ) ) - 1000    'max and min in brads
   PUT SPaddr, servo                      'save corrected
   IF ( servo <> servo(set) ) THEN   'is position different than the last time you sent?
      PUT SPaddr+set, servo            'it's different, save servo(pos) as servo(set)

      'HERE IS CONVERSION FROM BRADS TO 2us PSC steps -- THIS WORKS FINE
     'servopsc ends as word val to be offset and sent to PSC
      ' servo is limited position in brads
      ' first and last "64" are scalers for integer math prob. unneccessary given that ~474 2us steps = ~64 brads on my servos


      servopsc = servopsc * 64 + 32 / 64 * servo + 32 / 64    'convert to servo steps last sent, do it

      GET ServoRaAddr, ioByte          'ioByte is used as ramp 0 - 63
      GOSUB PSC_Write                        'parameters: ServoChan, ioByte = ramp, servopsc = Word position
      GET ServoFAddr, Word ioWord            'get flags
      ioWord.BIT0(ServoChan) = 0             'clear this servo channel's flag
      PUT ServoFAddr, Word ioWord            'save the flags
   ELSE
      GET ServoFAddr, Word ioWord            'you sent this position already, go check it, set flag, etc.
      IF ( ioWord.BIT0(ServoChan) = 0 ) THEN 'you sent it, see if you got there
         GOSUB PSC_Read                       'returns word servopsc as actual read position fro PSC, ioWord as 2us steps = 64 deg

      'HERE IS CONVERSION FROM 2us PSC steps to brads -- THIS IS OFF BY 1 BRAD SOMETIMES AND GIVES ME TROUBLE

       'servopsc is actual position report from PSC in 2us steps
       'ioWord is calibrated scaler for this servo = 2us to make 64 brads or 90 deg.
      ' servo(rd) will be converted position in brads 128 - 64 - 0
      ' first and last "512" are scalers for integer math

       servo(rd) = 512 * 64 + ( ioWord/2 ) / ioWord * servopsc + 256 / 512

       PUT SPaddr+rd, servo(rd)                    'save it
         GET SPaddr, servo(pos)                     'refresh orig. desired position because of variable setup in subs
         IF ( servo(rd) = servo(pos) ) THEN      'if they match, yer there
            GET ServoFAddr, Word ioWord         'so set this servochan flag
            ioWord.BIT0(ServoChan) = 1
            PUT ServoFAddr, Word ioWord          'save the flags
         ENDIF
      ENDIF
  ENDIF
  RETURN




P.S. -- I looked over Tracy Allen's examples of accurate long division, but I'm not sure that's the ideal solution here -- I'm getting pretty tight on code and variable space and this the only place in the project (so far, anyway) where I need to do this kind of accurate conversion. It's a real one-shot. But I'm very open to suggestions.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
When the going gets weird, the weird turn pro. -- HST

Post Edited (Zoot) : 9/12/2006 6:27:47 PM GMT

Comments

  • Tracy AllenTracy Allen Posts: 6,662
    edited 2006-09-13 07:54
    The problem could be simple roundoff error. There is quite a jump in resolution in going from brads to PSC steps and then back to brads again. With rounding off, there is bound to be an off by one error at some point. I guess I don't understand why the error is intolerable. Could the feedback simpy accept as a match the exact value or one off from that? Or is there a serious mechanical problem? And I don't understand what happens if there is a mismatch. If the feedback loop is closed, if the brads came up short, a few more pulses would be sent to the PSC until error=0.

    About the math in the formulas, I'm not sure about the complications. Are the two formulas basically the following...
    servopsc = servopsc * 64 + 32 / 64 * servo + 32 / 64
    becomes
    servopsc = servopsc * servo
    and
    servo(rd) = 512 * 64 + ( ioWord/2 ) / ioWord * servopsc + 256 / 512
    becomes...
    servo(rd) = 64 * servopsc / ioWord
    What is the expected range of values, and why the fancy stuff?

    ▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
    Tracy Allen
    www.emesystems.com
  • ZootZoot Posts: 2,227
    edited 2006-09-13 15:24
    Thanks for replying. In reverse order:

    - example expected values: PSC reported position of 750, offset to ~300 returns 64 brads (centered servo)
    Brads of 64 returns ~300 then offset to 750 to position servo

    - the fancy stuff is scaler for integer math -- the conversion from brads to PSC 2us steps is fine -- esp. given change in resolution.
    As I commented in my code, the first 64 is for scaling for integer math and is probably unneccessary given the change in resolution. But the conversion to 2us is fine.

    - the conversion from brads -- the first and last 512 are for integer math -- any multipliers smaller than 512 and the error goes way up; any value higher and I blow my 16bit limit

    - positioning the servo is fine, the problem is that I read back the ACTUAL position and compare to the sent position to see if servo has gone where I want (I use a bunch of finite state machines for sending my servos, then other states check the servos and associated sensors and parse resulting info into yet more state changes). So I might send a servo to position X (in brads) at a slow ramp speed; it might be 10 or 20 passes through the FSM before the reported position actually matches the sent position (which means I'm done).

    - so... if the reported position (converted to brads) is off from what was sent, I don't get a match. Now, in the real world, my error using the code above is always 0 or 1, and I'm actually running a version of the code now where I knock off the LSB before checking that servo(pos) = servo(rd), but that struck me as, well, inaccurate. Also, given the fairly course resolution of brads on a 180degree sonar sweep, being off by one brad in a reported position actually starts adding in errors when I do my trig on the dist/angle results. But if I'll have to live with possible errors of <=1 given rounding at these resolutions, then so be it.
    Here I'll just use "SomeScaler" to show constant for doing integer math on the conversions:

    'servopsc = final word in 2us steps for PSC, servo = byte in brads; ioWord = # of 2us steps to get 90 deg. of travel
    servopsc = ioWord * SomeScaler + 32 / 64 * servo + (SomeScaler/2) / SomeScaler  'this works fine enough, btw
    
    'servo(rd) = final reported position of servo in brads (byte); ioWord = # of 2us steps to get 90 deg. of travel; servopsc = position of servo as reported by PSC
    servo(rd) = SomeOthScaler * 64 + ( ioWord/2 ) / ioWord * servopsc + (SomeOthScaler/2) / 512
    
    



    One final comment: having all my servos ready to go in brads has greatly simplified my project -- if I tell any given servo to go 70, I know that servo (calibrated) will go to 6 brads CCW from center. And it means all the sent and reported positions are ready to plug into SIN, COS, ATN when I do my trig work.

    One other thought -- in looking over Mr. Lindsay's Ping)))Dar code, might I be better off using a conversion that is 1 brad = X 2us PSC steps? Instead of converting in 64 brads = X 2us PSC steps? I used 90deg. equivalents because I already had those calibrated values for each servo from older code.

    ▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
    When the going gets weird, the weird turn pro. -- HST

    Post Edited (Zoot) : 9/13/2006 3:50:24 PM GMT
  • Tracy AllenTracy Allen Posts: 6,662
    edited 2006-09-13 16:59
    Would the first formula,

    steps = servopsc * 64 + 32 / 64 * servo + 32 / 64

    simplify to:

    steps = K * servo / 64? ' K = steps per brad

    I used "steps" and constant "K" just for discussion. I understand you are reusing variables for different purposes.
    You suggested that the first factor of 64 is probably not necessary, and it looks like the "+32" in each case is meant for round-off. Unless I'm misunderstanding and the +32 has to do with the offset. I think it should be okay to do the calibration at 90 degrees and store that value.

    The problem is to get an exact match between that formula and an inverse formula.
    servo = steps/K ' K = steps per brad.
    Is that right? If the constant is in the denominator, there is not so much of a problem with integer math. The value 1/K can be converted to a constant for use with the ** operator. Precompute calibration constant M = 65536/K
    servo = steps ** M
    That would be much more accurate. But I might be way off base with understanding.

    ▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
    Tracy Allen
    www.emesystems.com
  • Tracy AllenTracy Allen Posts: 6,662
    edited 2006-09-13 17:03
    Would the first formula,

    steps = servopsc * 64 + 32 / 64 * servo + 32 / 64

    simplify to:

    steps = K * servo / 64? ' K = steps per brad

    I used "steps" and constant "K" just for discussion. I understand you are reusing variables for different purposes.
    You suggested that the first factor of 64 is probably not necessary, and it looks like the "+32" in each case is meant for round-off. Unless I'm misunderstanding and the +32 has to do with the offset. I think it should be okay to do the calibration at 90 degrees and store that value.

    The problem is to get an exact match between that formula and an inverse formula.

    servo = 64 * steps / K ' K = steps per brad.

    Is that right? If the constant is in the denominator, there is not so much of a problem with integer math. The value 1/K can be converted to a constant for use with the ** operator. Precompute calibration constant M = 65536/K

    servo = 64 * steps ** M

    That would be much more accurate. Depending on the value of K, it might be possible to roll part of the 64 into the M for still greater accuracy. But I might be way off base with understanding.

    ▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
    Tracy Allen
    www.emesystems.com
  • ZootZoot Posts: 2,227
    edited 2006-09-13 17:15
    said...
    "+32" in each case is meant for round-off."

    Correct.
    said...
    The problem is to get an exact match between that formula and an inverse formula.

    Correct.
    said...
    servo = 64 * steps / K ' K = steps per brad

    Would I still want to add in my (k/2) for rounding? e.g. servo = 64 * steps + (K/2) / K ' K = steps per brad
    said...
    Is that right? If the constant is in the denominator, there is not so much of a problem with integer math. The value 1/K can be converted to a constant for use with the ** operator. Precompute calibration constant M = 65536/K

    Actually, I did do this in my initial version -- this is also what Andy Lindsay does in Ping))dar but he did steps/brad not steps/64brads. I got greater discrepancies between the converted sent position and the returned converted position. But since I have my 90 degree = 64 brads calibrated figures I can set it up and try it that way. It reads cleaner that way, certainly.

    ▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
    When the going gets weird, the weird turn pro. -- HST
  • Bruce BatesBruce Bates Posts: 3,045
    edited 2006-09-13 17:36
    Gents -

    Since 2^5 = 32, might not shifting give you a more consistant result, wheter it's "rounded" properly or not,
    if that's the goal - consistant results?

    Regards,

    Bruce Bates

    ▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
    <!--StartFragment -->
  • ZootZoot Posts: 2,227
    edited 2006-09-13 17:58
    Bruce --

    I know what you're driving at, but not sure I follow. Shift what? When converting to steps from brads, e.g.

    steps = K << 6 * servo >> 6

    ??

    ▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
    When the going gets weird, the weird turn pro. -- HST
  • Tracy AllenTracy Allen Posts: 6,662
    edited 2006-09-13 19:46
    There are quite a few steps per brad. If the value returned from the psc and converted to brads is off by only one, it might need a small adjustment to make it right. Adjust by 1/2 brad at a time instead of 1 full brad, so the feedback could reduce the error to zero?

    >>Would I still want to add in my (k/2) for rounding? e.g. servo = 64 * steps + (K/2) / K ' K = steps per brad

    I'm all in favor of keeping the calculations simple. It is simpler to think of the formulae as
    steps = K * brads / 64
    and
    brads = 64 * steps / K
    The one-to-one mapping is pretty clear, but with the explicit +32 or +K/2 rounding in addition to the integer truncation, it gets more complicated to analyze. Maybe the consants K/64 and 64/K could both be stored for use with */ or ** operators, in order to minimize the integer trucation errors in both cases. If an offset is necessary I'd still prefer to keep it simple so the two functions are clearly related algebraically.
    steps = K * brads / 64 + offset
    and
    brads = 64 * (steps - offset) / K
    And use */ and ** to retain precision.

    ▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
    Tracy Allen
    www.emesystems.com
  • ZootZoot Posts: 2,227
    edited 2006-09-13 20:50
    The 1/2 brad idea is a good one -- and that'll fit in a byte at 0 - 255 half brads = 0 - 128 brads. I'll try this out tonight or tomorrow night along with storing my */ and ** vals. Thanks for the assistance.

    ▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
    When the going gets weird, the weird turn pro. -- HST
  • Bruce BatesBruce Bates Posts: 3,045
    edited 2006-09-14 04:38
    Zoot -

    That's the idea.

    Bruce

    ▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
    <!--StartFragment -->
Sign In or Register to comment.