Trig Functions?

A friend of mine has his Vex robot running on the crab drive kit, and he asked me to help him write a point-and-shoot style control program so that the robot moves in the direction the joystick is pointing. He has a potentiometer measuring how far the wheels are turned. Basically, I need a way to take the vectors of the x and y axes and turn that into an angle of the joystick and a magnitude from the center (the farther away from the center in any direction, the faster it goes).

Now, this wouldn’t be a complicated project on an FRC controller, but the Vex controller does not support floating-point, double, or long variables, so that leaves out the math.h library, which contains all of the trig functions necessary for 2D vector work. Is there another library around that I can use, or a low-processor usage way of deriving the trigonometric functions with integers? I mainly need arctangent (atan or preferably atan2) to figure out the angle of the joystick, and the magnitude I’m not worried about because it can be done with Pythagorean theorem. I’ve looked at the Maclauren series functions, but they still require long and float variables, and are hugely memory and processor intensive. Is there any way to do arctangent other than a lookup table?

Check out this article that contains information relating to floating point trigonometric functions using a PIC18F452 (which is similar to a PIC18F8520). Other alternatives are COORDIC Methods and using a floating point co-processor.

Couldn’t you use the x axis of the joystick to control the turning angle?

Imagine the joystick as a circle covering 360 degrees with the limits of the x axis as 0 degrees on the right and 180 degrees on the left. If you pick an angle between 181 degrees and 359 degrees and draw a line through the center, it will be the same as an angle between 1 and 179 degrees. You can then limit the travel of the turning angle for the crab drive to 0 thru 180 degrees. The y axis would control direction - forward or reverse and the magnitude would control the speed.

The program would look something like this for figuring out the turning angle

For o to 180 degress the turning angle = value of x axis.

For 181 to 269 degrees:

If y axis is greater than middle value and x axis is greater than middle value then turning angle = middle value - (value of x axis - middle value).

For 271 to 359 degrees:

If y axis is greater than middle value and x axis is less than middle value then turning angle = middle value + (value of x axis - middle value).

Sorry for the long post, but I hope this helps.

Yeah, I thought about doing that briefly, but consider this case: y = 0. You could have any value of X you want, and the angle will never change (0 and 180 are just flipping the direction of the motors for this program). Now that you’ve wrapped your head around that, the same can be applied to any angle pair. Let’s go with the 45* mark. Using a signed character variable, this could be any value between 1,1 and 127,127, as long as the two axes are the same value. So that doesn’t work because you have to adjust the “sensitivity” of the conversion between the x axis and the angle based on y. Well, that’s arctangent for you.

I happened to find a close approximation of the atan2() function using only integers (within an accuracy of about 10 degrees or .07 radians) using Google, which subsequently broke when I started searching other things (I broke Google!). It basically goes off of the ratios of subtracting x and y in different arrangements to closely mimic the shape of the arctangent curve without technically going through the same process. Here’s the code for the function:

int arctan2(int y, int x)
   int angle, abs_y = y;
   if(abs_y<0) abs_y=0-y;  //If y was negative negate it to get the absolute value

   if(x>=0) angle = (45 - (45 * (x - abs_y) / (x + abs_y)));
   else if(x<0) angle = (135 - (45 * (x + abs_y) / (abs_y - x)));

   if (y < 0) return(360-angle);     // negate if in quad III or IV
   else if(y >= 0) return(angle);

If you use this code as a function, of course, remember to prototype it at the top of your program. As you can see, it uses very simplistic-looking complex math like thingymajigs which I can’t understand tonight, as it is currently 3:12 AM. Thanks to everyone for their excellent suggestions and willingness to help me on my friend’s project!

I don’t think you understood what I was trying to explain, but I’m glad that you were able to find some equations on google to help you.

My idea didn’t actually calculate or use any angles. What I called “turning angle” was just the value from the joystick that the program would use to steer the crab drive using feedback from the potentiometer monitoring the steering.

The crab drive would only steer through 0 to 180 degrees because the the other directions could be achieved simply by changing the direction of the drive motors. It wouldn’t matter if y = 0 because that would be stop. I was just using the y axis of the joystick to control speed and direction of drive motors. The x axis would control the steering using the equations.

I guess I should have used something other than “turning angle” - sorry for the confusion. To figure everything out I imagined a circle going counter clockwise from 0 to 360 degrees while looking down on the joystick and robot. 0 degrees would on the x axis to your right. This was just a way to help me visualize the directions the robot would travel and to figure out the equations without using any angles.

An example would be going reverse diagonally to the left would be the same steering postion as going forward diagonally to the right. The only change is the direction the drive motors are spinning. Thus by just steering between 0 and 180 degrees you can re-create motion in any direction by changing the direction of the drive motors. Does this make sense?

Sometimes it’s kind of hard to explain things in a text message instead of in an actual conversation or without diagrams.

Anyway, good luck on your friend’s project!

That’s the easy way to program it, and that’s how he has his robot set up right now. What he wanted was point-and-shoot, where you take the angle of the joystick from horizontal to control the turning angle, and then the distance of the joystick from the center to be the speed of the motors. That way, whichever way you were pointing the joystick, the robot is going. It’s a lot more intuitive, and makes it better for demos, or just plain driving it around. Right now, you have to think about how far the wheels are turned and flip the motor direction manually, so there’s no possibility of contiguous movement. With this, if you lower the joystick past the acceptable range for the pot, it flips the wheels around automatically and reverses motor direction and you can keep driving.

I was trying to do a point-and-shoot system so that the robot would move whichever direction the joystick was pushed. I was just trying to do it without having to use any angles. That’s why I was comparing the x and y axis in my IF statements. The program would flip the wheels around and automatically reverse the motor direction for continous motion if needed - just like you said.

After your posts though, I got to thinking about it and I discovered a few faws in my idea. I might be able to figure out a way to fix them, but it would probably be easier to do it your way by calculating the angle (I think). I may try some experimenting on my own to see if I can it to work though.

I hope that you will post your code when you get it working. I’m sure a lot of us could use it - I know I would like to have it, especially if I can’t get something figured out myself.

I have code in WPILib that I THINK works, but I’m not quite sure. It assumes a lot of things, so I need to comment it a lot more, but I might be able to post it soon. Don’t count on it, it’s FIRST season, but I might be able to spare a few minutes. I’m not sure it will work correctly, but it compiled (first try, yeah!), so that’s a good sign. I’ll get it up whenever I finish tweaking it. Basically, he has a pot geared into the swivel so that the wheels can pivot 360* before having to flip over, but I haven’t gotten the limits programmed in yet because he wrote everything in RobotC and I’m having to convert back and forth between his values. Eventually, I’m going to convert this whole program into RobotC too so that he can use it and I’ll post them both here.

Could you try making a look-up table of trig functions in an array manually?

You can include math.h in both easyC V2 and Pro. This will allow you to do trig on the vex controller. Here is a reference

Unfortunately the 8-bit processor takes a long time to calculate the answer, so if you put it in a while loop and run it every time though it can cause sluggish performance. But, you can either use a timer and call the formula every 5-20ms or create an array and make a look up table. In FRC most teams created a lookup table on the PIC controller.

Is that float emulation specific to the EasyC compiler? I was under the impression that there was NO floating support WHAT SO EVER. I’m using Piklab (MPLab clone for Linux) with the C18 v. 2.4 compiler, and every time I try to use floats, it doesn’t run slowly, it just doesn’t work. At all. Everything that doesn’t revolve around the float works fine, but the float math doesn’t do anything. And yes, I use the correct explicit conversions.

But good to know, I’ll look into the floating emulation of EasyC and see if I can hack it into Piklab.

Hello all,

Attached is an EasyC program that does exactly what was originally asked for in this thread. It calculates the magnitude and direction of a joystick.

More detailed notes are listed in the program. But in short, the inputs are channel 1 and channel 2. The output is the length of the hypotenuse formed by the channels, and the direction the stick is pointed between 0 and 360 degrees. Outputs are printed to the EasyC terminal window.

The square root is calculated by means of successive approximations and the arctangent is approximated by the formal method within the range of accuracy you can achieve with a VEX controller. I’m sure you’ll find that its consistant and more than accurate enough.

Tested on VEX code v7, User20.01 and EasyC Ver

Sorry this took so long, it was more challenging than I realized.

KHall (3.51 KB)

Wow, sorry I’m getting back to you so late, I don’t get e-mail notifications of when people post. If you aren’t too busy, could you please post the straight C code from EasyC instead of the project files (trying to open them without EasyC gives me a bunch of null and end-of-file characters that make it impossible to read)? Thank you for your work on this, from what little I was able to glean from the gibberish looks nice.

Since I am here, I will unpack it for you…

#include "Main.h"

void main ( void )
      unsigned int SumOfSquares = 0; 
      int Seed = 400; 
      int Magnitude = 0; 
      int Count = 0; 
      int Channel1X = 127; 
      int Channel2Y = 127; 
      float X; 
      float Y; 
      float Angle; 
      float Ratio; 
      float Temp; 
      float Third; 
      int Quad; 
      int Y_GreaterThan_X = 0; // 0 = No 1 = Yes
      float Adjust = 0.8183; // Adjust for rounding errors (0.8 - 0.9)

      Wait ( 1000 ) ;
      // Program to calculate the magnitude and direction 
      // of a VEX Joystick.  The inputs are Channel 1 and 
      // Channel 2 from the VEX transmitter. 
      // The outputs are the magnitude of the distance 
      // the stick is moved from center and the angle in 360 degrees. 
      // The square root of the right triangle is calculated 
      // by means of sucessive approximations. 
      // The arctanget of X and Y is calculated by converting 
      // all readings to standard position and limiting the 
      // doman (input) to between 0 and 45 degress. 
      // VEX code v7, User v20.01 and EasyC Ver 
      // Author KHall.  Please support VEX and intelitek. 
      while ( 1 == 1 )
            // Get readings from the transmitter. 
            Channel1X = GetRxInput ( 1 , 1 ) ;
            Channel2Y = GetRxInput ( 1 , 2 ) ;
            // Convert readings to standard X,Y coordinates. 
            X = Channel1X  - 127 ;
            Y = (-1)*( Channel2Y  -127) ;
            // Compute the magnitude of the vector. 
            // Range based on 0 to Square root of 128^2 + 128^2 =181 
            SumOfSquares = (X * X) + ( Y * Y ) ;
            for ( Count = 0 ; Count < 8 ; Count ++ )
                  Magnitude  = ( Seed + ( SumOfSquares / Seed ))/2 ;
                  Seed = Magnitude  ;
            // Determine Which Quadrant the reading is in 
            // and convert the reading to standard position. 
            if ( X >= 0 && Y >= 0 )
                  Quad = 1 ;
            if ( X < 0 && Y >= 0 )
                  Quad = 2 ;
                  X = (-1) * X ;
            if ( X < 0 && Y <  0 )
                  Quad = 3 ;
                  X = (-1) * X ;
                  Y = (-1) * Y ;
            if ( X > 0 && Y <  0 )
                  Quad = 4 ;
                  Y = (-1) * Y ;
            // Determine if Y > X and if so,  
            // then make note of it and flip them 
            // to force the angle between 0 and 45 degrees. 
            if ( Y > X )
                  Y_GreaterThan_X = 1 ;
                  Temp = X ;
                  X = Y ;
                  Y = Temp ;
            // Prevent divide by zero. 
            if ( X == 0 )
                   X ++ ;
            // Compute the approximate arctangent of the angle 
            // in radians, then convert to degrees. 
            Ratio = (255*Y)/X ;
            Temp = Ratio/1020 ;
            Third = (Temp * Temp * Temp)/3 ;
            Third = (Third * 1020 )/255 ;
            Angle = ((Ratio/255 - Third) * 180/3.14159) ;
            Angle = (Angle* Adjust ) ; // Adjust for rounding errors
            // Adjust if angle was greater than 45 degrees. 
            if ( (Y_GreaterThan_X == 1 ) && (( Quad == 1) || (Quad == 3)) )
                  Angle = (90 - Angle) ;
            if ( (Y_GreaterThan_X == 0 ) && (( Quad == 2) || (Quad == 4)) )
                  Angle = 180 - (Angle - 90) ;
                  Angle = Angle - 180 ;
            // Adjust the computed angle to the correct 
            // value 0- 360 degress by adjusting for the quadrant 
            Angle = Angle + 90 * ( Quad -1) ;
            // Cleanup, reset variables 
            Seed = 400 ;
            Y_GreaterThan_X  = 0 ;
            // Produce output to terminal window of the results. 
            PrintToScreen ( "Magnitude = %d\n" , (int)Magnitude ) ;
            PrintToScreen ( "Angle is: %u\n" , (int)Angle ) ;
            PrintToScreen ( "\n" ) ;
            Wait ( 2000 ) ;