Turns out that mapping the motors is not that simple in C-programming, since our power function depended on the use of a square root, and the direction depended on finding arctan() to determine angle. I tried using math.h (even though it takes up a lot of space) and it took a whopping 5ms to calculate arctan() (sqrt was in that same range). We set out to make approximations that were based on simple polynomial expansions and would only take ints (not floating points).
For finding the square root, we do the following, where r (sqrt(x^2+y^2)) is the number we want to find the square root of (if you notice the first function will give us a dead-band, which we might want, since for values of x less than approximately 40, the int becomes truncated. Coefficients are based on the fact that the A/D converter will give us numbers between 0 and 255 for both x and y coordinates, which we translate to -127 to 127):
For finding the square root, we do the following, where r (sqrt(x^2+y^2)) is the number we want to find the square root of (if you notice the first function will give us a dead-band, which we might want, since for values of x less than approximately 40, the int becomes truncated. Coefficients are based on the fact that the A/D converter will give us numbers between 0 and 255 for both x and y coordinates, which we translate to -127 to 127):
For x < 1000
For x > 1000
For finding direction, instead of trying to be linear with respect to theta, we use cos, which gives us the same extreme limits, but gives us less sensitivity near the equilibrium position. This might or might not be an undesirable effect; we will have to see, but this is a lot easier to calculate. The graph below shows direction being linear with theta, and the difference of using cos.
