Thursday, April 14, 2011

Drive values solved... hooray trigonometry!

So I've been trying to decide how to determine the values to set the drive motors from the X-Y axis values that I'm sending from the controller.  When the stick is just moving in the Y direction, I want both wheels to go the same speed (forward or backward), when the stick is just moving in the X direction, I want both wheels to go the same speed, but in opposite directions (forward/backward or backward/forward), and when the stick is in some combination of the two axes, I want the values to interpolate between these extremes. 

My initial calculations caused some deadspots in the stick values where the droid wasn't moving in the direction that I was expecting (or it wasn't moving at all).  So I thought instead of having discrete cases to do certain things with the values, if I could find continous functions that I could feed in these inputs that would output the values I wanted, I wouldn't have to have a bunch of if-else statements.  So I set out to find these functions.

Today I was brainstorming and thought "maybe cyclical trig functions would help out", so I looked up the unit circle:

I got this picture from:
http://www.mathpeer.com/images/trig/unit_circle.gif
 






















Well, this isn't exactly helpful... I mean, full Y and no X should be both wheels forward... here, it would be full right, no left.  Then I put my head on my chin and thought some more... and while my head was tilted on its side, I finally got it... what if:


... I turned the unit circle a little bit (about 45 degrees).  Now, full Y and no X would give me the same value for both wheels (with sine and cosine).  What about my other conditions?  Well, full left X and no Y will give two values of equal magnitude, but opposite direction... and full right X and no Y gives the opposite effect... AND full backward Y and no X gives the same values, but both negative.  EUREKA!

So, my new process will be:

1) Input X and Y axis values from the controller (from 0 to 255)
2) Map these values so there are negative and positive values (ie: -200 to 200)
   (this will allow me to determine which quadrant of my new unit circle the stick is in)
3) Determine the angle from one of the axes using the X-Y values and arctangent (arctan(X/Y) = angle)
4) Assign cosine of this angle to one wheel, and sine of this angle to the other
   (these sine/cosine values will now act as percentages of SPEED, determined by how far from center the stick is [calculating the length of the hypotenuse, since X and Y act as the other two sides])
5) Multiply these assigned percentages to the speed and set the results to the wheels

Voila!  No if-else trees, just short pre-process calculations then set values and go.

In theory... I've only come up with these just now during my Thursday classes.  I'll test these ideas tomorrow and hopefully, that will be the end of the drive tweaking (and indeed, the end of the software development entirely).

No comments:

Post a Comment