Hexapod InverseKinemetics Problem

Other Hardware Robotics
3

a 2 minute refactor gave me 2 places where the code was doubtful, indicated with comments

I advice to give the functions a more meaningful name if possible to improve readabililty

Th ehypot function comes from - avr-libc: Modules = math.h (all can be used in Arduino)

#include "InverseKinemetics.h"

void RL3 (int i, Data *DataLine)
{
        int t = i*3;
	long x = DataLine->ServosPosition[2+t];
	if (x != 0) DataLine->l3 = hypot(DataLine->ServosPosition[0+t], x);
	else DataLine->l3 = DataLine->ServosPosition[0+t];
}

float ZX (int i, Data *DataLine)
{
  int t = i*3;
  RL3(i, DataLine);
  long x = DataLine->ServosPosition[2+t];
  long y = DataLine->ServosPosition[0+t];
  if ((x != 0) && (y != 0))                                // <<<<<<<<<<
    return atan(float(x) / float(y));
  else
    return 90; // Math Error Protection.
}

float TT (int i, Data *DataLine)
{
  int t = i*3;
  long x = DataLine->ServosPosition[0+t];
  long y = DataLine->ServosPosition[2+t];
  float x2 = x * x;
  float y2 = x * x;                         ////  you might meant index DataLine->ServosPosition[1+(i*3)] here ????
  float z2 = y * y;
  float s2 = DataLine->s * DataLine->s ;
  float l32 = DataLine->l3 * DataLine->l3;
  
  DataLine->s = sqrt(x2 + y2 + z2);

  float t1 = 0;

  if (x != 0) t1 = acos((s2 + y2 - l32) / (2 * DataLine->s * x));
  else t1 = acos(0.0);  // Math Error Protection.
    
  float a = (s2 + l1 * l1 - l2*l2);
  float b = (2 * DataLine->s * l1);
  float t2 = acos(a / b);

  return t1 + t2;
}

float LL (Data *DataLine)
{
  return acos(l1*l1 + l2*l2 - DataLine->s*DataLine->s) / (2 * l1 * l2));
}

void setleg (int i, Data *DataLine)
{
  int t = i*3;
  if (i >= 3)
  {
    DataLine->ServosAngles[0+t] = ((float)180 - ZX(i, DataLine));
    DataLine->ServosAngles[1+t] = ((float)180 - TT(i, DataLine));
    DataLine->ServosAngles[2+t] = LL(DataLine);
  }
  else
  {
    DataLine->ServosAngles[0+t] = ZX(i, DataLine);
    DataLine->ServosAngles[1+t] = TT(i, DataLine);
    DataLine->ServosAngles[2+t] = ((float)180 - LL(DataLine));
  }
}




void InverseKinemetics::IK(Data *DataLine)  
{
#ifdef DEBUG
  Serial.println("### InverseKinemetics->IK ###");
#endif

  for (int i=0; i<6; i++)
    setleg(i, DataLine);

#ifdef DEBUG
  for (int i=0; i<6; i++)
  {
    if (i != 0)
      Serial.print(", ");
    else
      Serial.print("### InverseKinemetics->ServosPosition's are: ");

    Serial.print(DataLine->ServosPosition[0+(i*3)]);
    Serial.print(", ");
    Serial.print(DataLine->ServosPosition[1+(i*3)]);
    Serial.print(", ");
    Serial.print(DataLine->ServosPosition[2+(i*3)]);
  }
  Serial.println(";");
#endif

#ifdef DEBUG
  for (int i=0; i<6; i++)
  {
    if (i != 0)
      Serial.print(", ");
    else
      Serial.print("### InverseKinemetics->Angles are: ");

    Serial.print(DataLine->ServosAngles[0+(i*3)]);
    Serial.print(", ");
    Serial.print(DataLine->ServosAngles[1+(i*3)]);
    Serial.print(", ");
    Serial.print(DataLine->ServosAngles[2+(i*3)]);
  }
  Serial.println(";");
#endif
}