Translating maths Formulas

Forum 2005-2010 (read only) Software Syntax & Programs
1

How do I translate a mathematic formula like this to still make the robot walk at myservo it should be
tri(x)
and at mysecondservo it should be
tri(x+alfa)
So translating maths formulas is the problem.

2

You decide the data type of the function "tri", you decide the data types of the parameters and you write a function.
e.g.

unsigned int tri (unsigned int x) {
  unsigned int result;
  //
  // you need to fill in this bit.
  //
  return result;
}
3

There are several pieces that help when you are using servos to make a walking robot. You will be doing a lot of atan calculations. Atan 2 returns more than just the base quadrants. And atan is expensive in terms of processing time. So here is an example of a quick atan2. It returns the whole range of results, not just the base quadrants (thus the atan2) and it is uses approximations to cut down on the processing time. The inaccuracy of this method is smaller than the inaccuracy of hobby level servos.

double quickAtan2(double num, double den)
{
int quadrant; 
double x =  (num/den);

if (abs(den) < .0001)
          {
          if ((num)>0) {return PI/2;}
          else {return 3*PI/2;}
          }
else {
          if      (num >= 0 && den > 0) {quadrant = 1;}
          else if (num >= 0 && den < 0) {quadrant = 2;}
          else if (num  < 0 && den < 0) {quadrant = 3;}
          else                          {quadrant = 4;}
         
          switch (quadrant)
          {
              case 1:
                 if (x<=1) {return x/(1+ 0.28*x*x);}
                 else {return (PI/2 - (x/(x*x + 0.28)));}
              break;
                         
              case 2:
              x=abs(x);         
                 if (x<=1) {return PI-x/(1+ 0.28*x*x);}
                 else {return (PI/2+(x/(x*x + 0.28)));}
              break;
             
              case 3:
                 if (x<=1) {return PI+x/(1+ 0.28*x*x);}
                 else {return 3*PI/2-(x/(x*x + 0.28));}
              break;

              case 4:   
              x=abs(x);               
                 if (x<=1) {return 2*PI-x/(1+ 0.28*x*x);}
                 else {return 3*PI/2+(x/(x*x + 0.28));}
              break;
          }
    }
}

Here is a circle-circle intercept routine, useful for calculating the position of a 'knee'.

  void CircleCircle(double x0, double y0, double r0, double x1, double y1, double r1, double *xSol1, double *ySol1, double *xSol2, double *ySol2, int *CCNumOfSolutions)
  {
    double a, dx, dy, d, h, rx, ry, x2, y2;
  
    dx = x1 - x0;    // dx and dy are the vertical and horizontal distances between the circle centers.
    dy = y1 - y0;
  
    d = sqrt(square(dy) + square(dx));   // Determine the straight-line distance between the centers.
  
    if (d > (r0 + r1))       { *CCNumOfSolutions = 0;return; }    // Circles do not intersect
  
    if (d < fabs(r0 - r1))   { *CCNumOfSolutions = 0; return;}    // One circle is contained in the other
  
    if (d == (r0 + r1))       { *CCNumOfSolutions = 1; }    // Circles are tangent
    
    // 'point 2' is the center of the two-circle union. Determine the distance from point 0 to point 2
  
    a = (square(r0) - square(r1) + square(d)) / (2.0 * d) ;
  
    // Determine the coordinates of point 2
  
    x2 = x0 + (dx * a/d);
    y2 = y0 + (dy * a/d);
  
    // Determine the distance from point 2 to either of the intersection points
  
    h = sqrt(square(r0) - square(a));
  
    // Now determine the offsets of the intersection points from point 2
  
    rx = -dy * (h/d);
    ry = dx * (h/d);
  
    *xSol1 = x2 + rx;
    *xSol2 = x2 - rx;
    *ySol1 = y2 + ry;
    *ySol2 = y2 - ry;
    *CCNumOfSolutions = 2;
  }

I guess I should warn you that I am relatively new to arduino programming. I could be doing horrible things with pointers and such. But my robot Does walk. :slight_smile: