Sine Law and servo rotation issues...

Cobalt Delta · 2008-06-23 14:55

Hello.
I'm using a BOE-Bot Board of Education Revision C (Stamp model BS2), and trying to write a program that will involve the calculation of angles based on the Laws of Sine and Cosine. Writing a Cosine Law is simple; it looks like this in the program.

VAR = SQR(a^2 + b^2 - 2ab COS C)

However, the Law of Sines requires an arcsine to work...

SinB/b = SinC/c -> B = arcsine (bsinC/c)

You see? Basically, I need to know the command for calculating an inverse sine in the BASIC Stamp Program.

Post Edited (Cobalt Delta) : 6/26/2008 12:56:02 AM GMT

Mike Green · 2008-06-23 15:13

The Basic Stamp does not have an arcsine function/operator. It does have an arctangent operator (ATN) and you can use that to compute the arcsine. Keep in mind that the angles are in binary radians (see the PBasic manual for an explanation). Do check out EME Systems' website for discussions of Stamp math (www.emesystems.com/BS2index.htm#math).

Beau Schwabe · 2008-06-23 15:18

While the BS2 does not have an Arcsine function, there is a successive approximation solution that can be found towards the bottom of this link.

http://forums.parallax.com/attachment.php?attachmentid=40342

Also, depending on your application requirements, you might be able to re-work your formula using the ATAN (Arctangent) and/or the HYP (Hypotenuse) functions, which are within the BS2 abilities.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Beau Schwabe

IC Layout Engineer
Parallax, Inc.

Cobalt Delta · 2008-06-23 17:47

Strange. I Googled an Arcsin calculation algorithm and came up with this...

One of the posts mentions how you could calculate Arcsin based on this formula:

ArcSin(x) = ArcTan (x / sqrt(1 - sqr (X)))

Attempting to use the ATN function in PBASIC, I am met with an error message during syntax check that it expects a unary operator... Which doesn't quite make sense to me if Arctan is in the same basic family as Sine and Cosine. ATAN reports that it is an undefined symbol.

Also, I am very much a novice at PBASIC programming. If anyone could clear up how I could effectively write the formula in correct PBASIC syntax, I'd greatly appreciate it...

Mike Green · 2008-06-23 18:00

The correct syntax is "ATN <expr>".

Before you get too far, remember that all arithmetic in the Stamps is integer-only and that it's 16-bit arithmetic with a range of -32768 to 32767. Many of these formulas won't work on a Stamp because there's no fractional part and the range is limited. There are often ways around these limitations in specific circumstances, but you can't just throw these formulas in without a careful understanding of the range of values and the number of decimal places involved for each arithmetic operation.

Cobalt Delta · 2008-06-23 18:58

...Hm, that might be a problem then. I did use the syntax correctly, I think... Here's a copy/paste of the small program I was using to calculate the formula.

A lot of values are assumed (They'll be accurately defined once the rest of the program is written, but for the purpose of testing this aspect, I can't be bothered to run the entire thing), and I am taking into consideration (To the best of my limited comprehension, at least! XD) that measurements are in brads. 2 is a constant to account for the discrepancy between the wheels' pivot point, and the sensor's pivoting point.

' {$STAMP BS2}
' {$PBASIC 2.5}

lol VAR Word
botdistance VAR Word
scandistance VAR Word
scanangle VAR Byte

scandistance = 32
scanangle = 14
scanangle = 128-scanangle 'Finds the supplement of the scanned angle.


botdistance = SQR(2^2 + scandistance^2 - 2 * 2 * scandistance * COS scanangle)
lol = ATN (((scandistance * SIN scanangle)/(botdist)) / SQR(1 - ((scandistance * SIN scanangle) / (botdist))^2)

DEBUG ? lol
END

The ATN expression is based on this trigonometric identity:
tan(sin^-1(x)) = (x / SQRT (1 - x²))
Therefore...
sin^-1(x) = tan^-1 (x / SQRT (1 - x²))
Since (x) is the equation bCosC/c, or scandistance times the cosine of scanangle divided by botdistance, it turns out the way you see it in the above example.

It still whines during syntax check about expecting a unary operator though...

Alternatively, I could add another triangle on top of the one formed by the scanner-object distance, the robot-object distance, and the robot-scanner distance. That way, I'd have a right triangle to work with, and the robot-object distance would form the hypotenuse of this new triangle... Though, honestly, now that this problem has presented itself before me, I don't feel like taking any alternatives... Darn stubborn pride...

Mike Green · 2008-06-23 19:25

It's complaining because there's a parenthesis imbalance and there is more than one operand on the right side of the operator.

It won't do you any good (other than false pride) to write a correct formula that doesn't produce a correct answer.

Cobalt Delta · 2008-06-23 20:13

Well, you do have a point there, and it seems like the program isn't working out too well. I could always make it perform the arctan operation on another line...

By adding another step and another variable to serve as the arctangent denominator, I figure I could possibly bypass this problem. No such luck. Any ideas what I'm doing wrong...?

' {$STAMP BS2}
' {$PBASIC 2.5}

lol VAR Word
hai VAR Word
botdist VAR Word
ArcSine VAR Word


botdist = SQR(2^2 + 32^2 - 2 * 2 * 32 * COS 12)     'Law of cosines
lol = ((32 * SIN 12)/(botdist))                     'Law of sines / trig identity part one - numerator
hai = (SQR(1 - ((32 * SIN 12) / (botdist))^2))      'Law of sines / trig identity part two - denominator
ArcSine = ATN (lol/hai)                                 'Perform Arctangent operation on numerator and denominator

DEBUG ? ArcSine

end

ATN still generates an error message regarding expectancy of a unary operator.

EDIT:
Upon crunching some more numbers, the difference seems to be too negligible to actually affect the angle by any integers. My other option was to move the robot forward by 4cm (The distance between the scanner and the wheels), then have it turn to the same degree measurement as the scanner. Having messed around literally all day with this trigonometry problem, I think I'm ready to lay it to rest and go with the simpler solution, as much as that frustrates me. Sometimes though, you have to go with the faster solution that still yields satisfactory results... It would actually turn out more accurate, too, given that the BASIC Stamps still operate in integer math...
Oh well.
Mike, I'd like to thank you profusely for all the help you offered. I really appreciate it!

Post Edited (Cobalt Delta) : 6/23/2008 8:30:08 PM GMT

MSDTech · 2008-06-23 23:29

The problem is how you are using the ATN operator. The syntax is: brads = xCoord ATN yCoord

Mike Green · 2008-06-24 00:04

Thanks MSDTech.· I misread the manual.
·

Cobalt Delta · 2008-06-24 07:54

So basically, instead of writing it arcsine = ATN (((scandistance * SIN scanangle)/(botdist)) / SQR(1 - ((scandistance * SIN scanangle) / (botdist))^2),

I'd need to write it

arcsine = ((scandistance * SIN scanangle)/(botdist)) ATN SQR(1 - ((scandistance * SIN scanangle) / (botdist))^2)

...Actually, now that I know what I did wrong, it might actually still be feasible to carry out the calculations as I'd originally planned. If I don't end up with any strange angles as my result, and the result is a reasonable estimate of slightly less than the detected scan angle, then that might just be it.

Thanks indeed, MSDTech!

Tracy Allen · 2008-06-24 15:10

Cobalt,

There are several other problems with that formula.

The ^ operator in PBASIC is XOR, not power. So x^2 means is exclusive or, not x squared. You want x*x.

The unity as in SQR (1 - (...)) has to be represented as a large integer, and the quantity (...) also has to be scaled to that same number as its maximum. Same thing for the number that comes to the left of the ATN. For example, unity might be represented as 128, and unity squared would then be 16384, and the quantity (...) would have to be similarly scaled. That is integer math for you.

Calculation of quantities like x * y / z such as you have in the above formula can be hard to do on the Stamp because of loss of precision in the integer division on one hand, and integer overflow on the other hand. Binary long division is slower, but it can maintain precision.

Here is a: CORDIC arctan function for the Stamp. Again, it is slower but has higher precision than the ATN function.

Given the roughly 8 bit resolution of SIN and ATN and SQR, there is going to be a lot of accumulation of error. There is a tradeoff between speed and precision.

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

MSDTech · 2008-06-24 15:59

If you are going to be doing a lot of these calculations and would like better precision, one alternative is to add the floating point coprocessor to the system. For information check:

http://www.parallax.com/Store/Components/AllIntegratedCircuits/tabid/154/CategoryID/31/List/0/SortField/0/catpageindex/2/Level/a/ProductID/244/Default.aspx

This processor does handle all of the trig functions and with its IDE is very simple to use. You just key in the formula and it will write all the BS2 code that you can cut and paste into the Basic Stamp IDE. I now keep several in my parts box for the occassions where I need more math processing power. It also has a debugging tool which allows you to define the formula as a funtion stored in the math co-processor.

Cobalt Delta · 2008-06-26 00:53

Thanks for clearing that up, Tracy. I never bothered to think that ^ was anything other than an exponent denotation.
Regarding the floating point coprocessor, if it were a solo project that I was doing for kicks, I would highly consider buying it... However, I'm actually writing this program as part of something called the TRIE program, which is sponsored by NASA, and designed to give prospective engineering students a glimpse at the activities and responsibilities of engineers of different disciplines, and my other group members decided that I would be the one to write the majority of the object detection code. What this means in shorter terms is that I'm constrained to the materials available to the program, with MAYBE ONE slight exception if the need is very great. Considering I have found an alternative to the trigonometric calculations, I don't think the instructors would be too keen on buying another component for a discarded segment of code. If I ever purchase one of these kits for myself though, I'll strongly consider it!

Unfortunately, the other group members of my project told me to cease and desist - that I was wasting too much time on the calculation! The final presentation of the program is due for two days from now, so fine-tuning that portion of the code was something they said I'd have to do on my own time... And actually, they seemed to be correct... It turned out to be simpler and more effective to roll the robot 4cm forward to compensate for the distance between the scanner's servo and the point where the wheels would pivot...

However, this brought on a new problem (Which would have arisen even if I had perfected the trig coding...)

The Parallax Continuous Rotation Servos (I don't have a model number, unfortunately, but I'm fairly sure you all know what I'm referring to.) tend to be very imprecise, and mathematical functions to scale their rotation distance seem nonexistent. For example, one might reason that doubling the duration of a FOR TO loop would cause the servos to run twice the distance for twice as long... Unfortunately, this is not the case.

My problem may be unique and potentially unsolvable given the nature of the servos I've "chosen to use" (read: gotten stuck with due to a very low budget...), but I need to know if it's possible to set a FOR TO loop to rotate two servos with wheels on them precisely one degree per loop.

The reason for this is that an ultrasonic sensor will detect the object closest to it by rotating 180 degrees (I actually DID get THAT squared away in a FOR counter = 0 TO 180 loop), write the degree it found the object at to a variable, and then rotate the actual Boe-Bot to that same degree (While compensating for the fact that the scanner starts at the Boe-Bot's 270-degree position by calculating the complementary angle for a measurement less than 90, and subtracting 90 for values greater than 90...).

I'm sure a lot of external influences, such as battery life, the weight of the robot (and its center of gravity) and the material it rotates on will have great effect on the servos, but I'm mainly looking for the opinions of those more experienced than myself, and any tips or suggestions you all may have. My primary focus is to get the servos to rotate a precise amount (one degree) for each loop and be able to scale that to any value the sensor may pick up.

Thanks again!

Post Edited (Cobalt Delta) : 6/26/2008 4:31:53 AM GMT

MSDTech · 2008-06-26 12:20

What, exactly, is the overall problem you are trying to solve? After reading your last post, I suspect it is to detect and move to the closest object.

Cobalt Delta · 2008-06-26 23:58

You're absolutely correct. In addition to that, the program will also count the number of edges it detects. However, I also noticed another problem - the ultrasonic scanner seems to become more imprecise as objects are placed farther away, which doesn't surprise me now that I've stopped to think about how it operates - the sound waves spread.

MSDTech · 2008-06-27 02:52

If that is the problem, you may be making this much more complex than it really needs to be. A simple thought experiment. You are placed in an empty room with a single door and told to leave the room. You would look around to find the door - the only thing you need to know is whether the door is to your left or right. You then start turning in that direction while watching the door. As you turn you are just checking "Is it to my left or right". You stop turning·when you are pointing close to the right direction and start moving forward. You start to move toward the door, again you are still checking "Is it to my left or right". If you see you are veering away from the doors direction you again turn the appropriate direction. This process repeats until you reach the door. By using your eyes as feedback during the maneuver, you can adjust for all the unknows.

You can do the same thing in your program code. Determine if the object is to the left or right of the robots current heading. You don't really need to know how far to the left or right, just·which direction. Use the sensor to provide feedback as you·turn to determine when you are pointing in close to the right direction to start moving toward the object. As you move toward the object periodically verify the bearing of the object and make a "mid-course correction" if necessary. Keep in mind, you may use a full 180 degree sweep to determine the direction of the initial location of the object, but subsequent checks won't need as wide a sweep.

Sine Law and servo rotation issues...

Comments