Calculating Angle from baseline (theta) from Two Motors/Servos Whatever

Zoot

I'm not sure if this as Stamp question or a basic Trig question -- but I am using as BS2P, so here goes. I have several different projects where I need to calculate the differential angle from a baseline based on two motors, or detectors, or whatever. For example, I might have two motors on a differentially steered platform, with speeds from 0-128-255 (R-Stop-F). What I want to do is calcuate the angle offset from 0 degrees (a straigh line path) based on the speeds, but danged if I can figure it out. e.g.:

Motor1(speed) = 0 (full reverse)
Motor2(speed) = 255 (full forward)
would be an angle of -90

Motor1(speed) = 255 (full f)
Motor2(speed) = 0 (full r)
would be an angle of 90

Motor1(speed) = 138
Motor2(speed) = 118
would also be -90

Motor1(speed) = 255 (full f)
Motor2(speed = 128 (stop)
would be -45 degrees

etc. ad nauseum

The actual effective speed of the platform doesn't matter -- the ratio of the normalized speeds to each other does. Also, it's easy enough to add (or subtract) offsets to the motor/sensor readings in my apps if it's easier to work with numbers like -127 to 127 or whatever.

I want to use PBASIC code in some these projects to drive servos that essentially "follow" the angle of turn, like a head that looks where it is going.

I've seen lots of good examples of calculating turn radius and such with differentially steered platforms, but nothing that I found helpful for this purpose. Again, the trig required is probably giving more problems than the work involved in fractions in PBASIC, so any help is greatly appreciated.

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

Franklin

Would your logic work only if both motors were under the same load and you knew their speed and length of time they were powered?

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
- Stephen

Zoot

If you mean some kind of real-world encoder, eventually, yeah, but it doesn't matter if the SENT speeds or positions are accurate; let's say they are. I still am not sure of the trig nor code moves to translate two differential values into an angle. Even if it weren't motor loads, say you sent two PWM values to two LEDs -- it would be cool to know that angle represented by the *relative* difference between the two so you could then, say, position a servo to look at the angle represented by the difference in the light values. Etc. etc.

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

Andy Lindsay (Parallax)

I just posted an application example that includes and explains these kind of calculations and maneuvers.

Go to the Stamps in Class forum, and open the blue Stamps in Class "Mini Projects" sticky thread at the top of the topics list. Then, follow the two links under the Boe-Bot + Ping))) Ultrasonic Rangefinder heading:

·- Ping)))Dar - a Radar Style Display
·- Scan for and Go to Closest Object

Notes:

(1) Each post is just a short excerpt, but each post also has a link to the full PDF document download.

(2) The examples are for a BASIC Stamp 2, but you can easily modify the PULSOUT and PULSIN commands' duration arguments to work with the BS2p with the help of either the BASIC Stamp manual or the BASIC Stamp Editor's online help.

Post Edited (Andy Lindsay (Parallax)) : 6/10/2006 10:13:40 PM GMT

Zoot

Andy -- I had just finished going through the Ping)))Dar and Finding Closest Object materials when I saw your post. Excellent write-up and I've already starting using the code for having my bi-nocular PINGed robot pan&tilt both "eyes" to an object that is closest.

However, for my purposes there is still one thing I'm not clear on -- getting my first theta (angle) based on my motor speeds.

If I reverse some of the processes you used, I would be *starting* with the pulse-to-angle count set up for the Boe-bot. In other words, Find Closest Object works it's way through the measurements but in the final output, sends the correct number of pulses that corresponds to the desired angle of turn. In my case, I don't have a turn-increment count to START with that would let me work back to an angle for my servo.

I could do a lookup table for 10-20 speed-difference pairs to deliver an angle -- but that seems inelegant. Am I making sense?

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

Andy Lindsay (Parallax)

Zoot,

Your robot with two Ping))) rangefinders sounds like too much fun.

-- General Info --

Regardless of the setup, the main thing to do is draw out a sample situation on a piece of paper, and solve it as a geometry/trig problem. Most of your solutions will involve converting from polar to Cartesian coordinates, making a few calculations, and then converting back to polar. Once you've got it figured out on paper, it should be doable with PBASIC. Cartesian to polar conversion has always been straightforward with the ATN and HYP commands. ATN gives you the angle, and HYP gives you the distance. The Polar to Cartesian conversion used to be a stumbling block. Now, with the Polar_To_Cartesian subroutine, it's just a matter of setting the values of the distance and angle variables before calling the subroutine, and it places the conversion results in the x and y variables.

Now, if you are turning your robot and trying to measure the angle the robot turned, there are several approaches. The simplest approach is to calibration your code so that each pulse causes a 1 brad robot rotation. Although it is used in the Finding the Closest Object material for controlling the turn-in-place angle, it's just as easy to use it to measure a turn. All you have to do is count the number of pulses that are delivered before your robot stops turning, and that's the number of brads it rotated. If you need to turn at more than one speed, you can also calibrate your program so that the robot does a 360-degree turn, then you know that every turn-in-place pulse corresponds to two brads of rotation.

-- Your question --

I'm not sure I understand the situation yet. I'm assuming you are trying to solve the same problem on the fly. In other words, you want to always be approaching the object as you are doing the calculations. If this is the case, I would try to choose some motor speeds that give me known arcs. For the on-paper solution, I would start solving the geometry problems iteratively, and watch how each measurement changes. For example, if you are sweeping the Ping))) rangefinder as the robot approaches the object at 8 inches per second, the y-axis value is decreasing by 1 inch per second, which can be further broken down into inches per servo pulse. If the Ping))) sweep increment between each drive wheel servo pulse is also a predictable value, a subroutine can subtract that distance the predicted object's distance from the actual distance measurement for on the fly corrections.

With on the fly stuff, the code becomes increasingly more concerned with incremental changes between each measurement and much less concerned with the actual location of the object. Consider the IR following Boe-Bot from Robotics with the Boe-Bot (pdf), chapter 8. The Boe-Bot reliably points itself at and maintains a constant distance from a target object that you can move around. All the program does is calculate the difference between the actual and desired distances of two IR distance measurements. Then, it calculates servo drives based on this difference between desired and actual measurements. In control system speak, this difference is called the error. Once you know the error, there are three common adjustments that are made to the drive, and some of each of the three ingredients tends to be the most effective: (1) drive the motor proportionally to the error, (2) keep a running total of all the errors and drive the motors based on taht running total, and (3) measure the difference between the current and previous measurements and drive based on that. Methods 1, 2, and 3 are referred to as proportional (P), integral (I), and derivative (D) control, and there's more about BASIC Stamp PID control in this post: PID Control Intro with the BASIC Stamp.

Andy

Post Edited (Andy Lindsay (Parallax)) : 6/11/2006 6:48:37 PM GMT

Zoot

Andy -- thanks for your help! Taking things slightly out of order:

- I've actually got a PID engine set up in one of my BS2p40 slots just for parsing, storing and delivering PID calculations. I still have some tuning to do, but I've got that code and concept in hand -- I think. Most of the structure is based on the various PID write-ups here at the forums.

- I've got my polar/cartesian on the Stamp down -- thanks to your most recent Stamps in Class articles. I learned polar/cartesian work pretty well in the Flash authoring environment, but that's PostScript -- with 10 digits of floating point numbers and regular ol' radians. Hacking the same into integer math and brads for the Stamp was tricky till I checked out your "Finding Closest Object" code and documentation.

I also have some Stamp code that works for figuring turn radius based on two motor speeds and axle width -- over time -- but that's not the same as ... my dilemma -- I have graphed it out on paper, so I know what I need to do, but I can't really figure out how to do it on the Stamp. What I'm trying to do is determine the angle of, I dunno, call it the numeric deviation from 0 of ANY two differential numbers.

Say I had two numbers that could range from -10 to +10

If the first number is +10 and the other is -10, that would be a "spin" or in this case, let's say -90 degrees.

If the first number is -10 and the other is +10 that would be +90 degrees.

If the first number is -1 and the other is 1 that would ALSO be +90 degrees because what matters here is the RATIO between the two normalized numbers -- in this case -1 again.

If I were do this in *signed floating point math*, I would normalize both numbers to the range and then just divide 'em after accounting for direction and a possible zero divisor. Really, the fact that it might be angle and speed isn't as relevant. I guess I'm really looking to map the relative ratio of the two numbers to some other scale. If I'm still not making sense, my apologies. I'm sure the fact that I can't even frame my question properly is a sign of my truly muddled thinking at this point

- I'll put pix and video and code from my bi-nocular Ping)))ed bot up in the projects forum soon -- when I have the code cleaned up enough that I'm not totally embarrassed, but there's one photo below. I'm currently working on true, uh, parallax tracking for this project, but that's a topic for another thread in the robotics forum probably...

Also attached is my PID slot program. This is designed to run in one program slot. It is called and returns via a task manager (as if it was a big subroutine). It uses EE data for constants. I can probably cut this sucker down in size, but am waiting to optimize code until I have everything working the way I want the first time around. For those that may be unfamiliar with multi-slot task managers, see some of Jon Williams' write-ups in N/V as well as Tracy Allen's write up at his site: emesystems.com/BS2SX.htm#Crossbank. My program doesn't use "global" variables for slot tasks (I use SPRAM), but you'll probably get the drift. This is only my second larger-scale Stamp project on a BS2P, so please forgive any inelegant coding. I've been truly humbled by all the spare, clean code I see at the forums and in the tutorials.

FYI all -- there is some good reading on differential steering motion control here: rossum.sourceforge.net/papers/DiffSteer/

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

Andy Lindsay (Parallax)

WOW!!! That's a cool robot and a great looking program. You're making excellent use of both the BS2p's features as well as the existing knowledge base for PBASIC. When you get this robot dialed, it would be great if you'd post some clips. Since this is what you're doing with the BASIC Stamp, I can't wait to see what you'll do with a Propeller Microcontroller (a floating point object is just one of its many virtues).

With BASIC Stamp modules, there are two possibilities that I know of for getting angles from the coordinates you described. The first is the low-res version, the ATN operator. So long as the x and y values fall in the -127 to + 127 range, it will do the correct calculation. For floating point calculations, the uM-FPU Floating Point Coprocessor http://www.parallax.com/detail.asp?product_id=604-00030 is good. The guy who designed it did a really nice job optimizing its functionality and protocol with speed in mind, and he put a lot of thought into making sure the extra processing time communicating with his device is minimized.

Tracy Allen

Cool indeed! Hero class.

I've written up Stamp CORDIC algorithms that give better resolution than the Stamps own SIN, COS, ATN and HYP operators. Posted at www.emesys.com/BS2mathC.htm

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Tracy Allen
www.emesystems.com

Zoot

Thanks for the props! The bot actually has a fine maple "table-top" that isn't on in the pix -- it's basically a big rolling coffee table. In any case, duh, ATN will do what I need... I don't why I didn't think of it. Esp. because for this app, my MD22 motor controller handles speeds of 0-255 or -127 to 127, and for other purposes I can easily map to that. Thank you thank you!

I will definitely put clips/pix/code up in the projects forum when I've got the bot where I want it.

And, yeah, I've been watching the dev. of Propeller with *great* interest for many months, and I'm very itchy to get my hands on one, but I can't get one to play with till I buy an XP Windows machine -- we're pretty much all Mac here......

For FP I looked at the FPC chip you mentioned and decided it would be better to switch over to a Javelin or a Propeller than to use precious Stamp space on interfacing with the FPC.

Thanks again. I've said it before, but working with Parallax products and especially the Parallax community has been one of the *great* pleasures I've had over the last year or so.

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

Zoot

OK -- now I know I am dense and muddled

ATN won't work, or at least, I don't see how to make it work.... I don't have a cartesian X and Y vector to get an angle from -- the X and Y would be a result of the ratio/mix of my two numbers to start with. Neither of which are actually my X and Y. It's the normalized ratio between the two that could give me X and Y or an angle map or whatever. If I had that, I'd be done I am so open to suggestions.

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

Andy Lindsay (Parallax)

Earlier, we were talking about having numbers such as +10 and -10 returning -90 degrees and -10 and +10 returning +90 degrees. Hmmm, yeah, ATN gives you -90 degrees for 0 and -10 and +90 degrees for 0 and +10. +10 and -10 should give you -45 degrees and -10 and + 10 should give you 135 degrees. So, how about subtracting 45 degrees from your result all the time? Will that work?

Zoot

Andy --

I *think* that would work. I'll test it out tonight and see. I think part of my problem here is not understanding my problem. To give a real world example -- I've got a Boe-bot with a "head" on a servo. Or I've got a servo mounted flashlight on a differential steered platform. Or an animatronic prop with a servo driven "hand".

What I want to do really is return what I might call a "perceptual" angle -- I want the flashlight or head or hand to "look" where the platform is going. For argument's sake, if you're in a full spin CW you'd want an angle of say -90. Full sping CCW would be 90. Both motors same speed would be 0, regardless of "actual" speed. Of course, you might want to map to 180 so that your light could look *behind* you when you are going backwards, or map to 127 so you can use brads, etc., but I think you're clear on that.

I think where I am confused about using ATN is that it seems like my two motor speeds are not really X and Y values in a meaningful way? but it sounds like you are saying it should work anyway if I correct the output to my zero point?

In any case, I came up with this code that may work -- I won't have a chance to tokenize this stuff until tonight, but regardless, this will probably gives an idea of what I'm shooting for. This seems real kludgey though on larger input/output ranges there would be potential scaling problems given 16 bit signed words. I'm also not sure about the rounding when the signed output is rescaled. Do I have it right that the amount you add is half of your scale? Again, thanks for all the help. Speaking as a non-engineer, I sure wish I remembered all my high school trig and calculus better... turns out I really would end up using it!

mn       CON  0           ' min of input range
ctr      CON  128         ' center (neutral or 0 point) of input range
mx       CON  255         ' max of input range
otpt     CON  90          ' output will be mapped to +/- this value e.g. -90 to 90 or you could do -127 to 127, etc
intscale CON  100                           ' scale for handling integer math, make sure not to exceed 16 signed bits

angle    VAR  Word
s0                            VAR  Word
S1       VAR  Word
Sign     VAR  Bit

' these aren't necessary as s0 and s1 could do all the work,
' but it makes it easier to see in this example

s0 = 128
s1 = 255
GOSUB Calc_Diff_Angle
GOSUB Debug_Results
' deno = .5
' nume = 1
' angle = 45 ish

s0 = 192
s1 = 255
GOSUB Calc_Diff_Angle
GOSUB Debug_Results
' deno = .75
' nume = 1
' now angle = 67.5 ish

s0 = 255
s1 = 64
GOSUB Calc_Diff_Angle
GOSUB Debug_Results
' deno = 1
' nume = .25
' now angle = -67 ish

s0 = 128
s1 = 128
GOSUB Calc_Diff_Angle
GOSUB Debug_Results
' deno = .5
' nume = .5
' now angle = 0

s0 = 100
s1 = 100
GOSUB Calc_Diff_Angle
GOSUB Debug_Results
' deno = .39 ish
' nume = .39 ish
' now angle = 0

s0 = 223
s1 = 192
GOSUB Calc_Diff_Angle
GOSUB Debug_Results
' deno = .875 or 87 integer
' nume = .753 or 75 integer
' now angle = -1080 before scaling; -11 integer; -10.8 floating

Calc_Diff_Angle:
nume = ( s1 * intscale ) / ( mx - mn )            ' normalized to range, presume symmetry
deno = ( s0 * intscale ) / ( mx - mn )            ' ditto
angle = ( nume - deno )  * otpt                   ' get normalized diff and multp by +/- range 
Sign = angle.BIT15                                ' maintain sign and rescale
angle = ABS angle + 50 / intscale                 ' handle proper rounding ? I think this is right for rounding
IF Sign = 1 THEN angle = -angle
RETURN

Debug_Results:
DEBUG "s0    :", SDEC s0, CLREOL, CR
DEBUG "s1    :", SDEC s1, CLREOL, CR
DEBUG "angle :", SDEC angle, CLREOL, CR
RETURN

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