Converting Brads to PSC 2us Steps and Back Again
Zoot
Posts: 2,227
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.
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
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
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
- 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:
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
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
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
Correct.
Correct.
Would I still want to add in my (k/2) for rounding? e.g. servo = 64 * steps + (K/2) / K ' K = steps per brad
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
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 -->
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
>>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
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
When the going gets weird, the weird turn pro. -- HST
That's the idea.
Bruce
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
<!--StartFragment -->