FireFly 2012: the robot code as an Arduino library 'hexapod' (v3.1): IK, SSC-32, IR, ultrasonic, GPS, 7-segment

From Electronics\Hexapod\Arduino library 2012.

2011-firefly-mega-ssc32/firmware/2012-library/hexapod/InverseKinemetics.cpp

@@ -0,0 +1,150 @@
+#include "InverseKinemetics.h"
+//  V3.0
+
+//Variables
+boolean error;
+
+void RL3 (int i, Data *DataLine)
+{
+  float x = DataLine->ServoData[i].x;
+  float z = DataLine->ServoData[i].z;
+
+  if ((x == 0) && (z == 0)) 
+    DataLine->l3 = 0;
+  else DataLine->l3 = hypot(z, x);
+
+  if (x < 0)
+    DataLine->l3 = DataLine->l3 * -1;
+}
+
+float O1 (int i, Data *DataLine)
+{
+  RL3(i, DataLine);
+
+  float x = DataLine->ServoData[i].x;
+  float z = DataLine->ServoData[i].z;
+  float r;
+
+  if ((x != 0) & (z != 0)){
+    double a = (z / x);
+    return atan(a);
+  }
+  else
+    return atan(0.0); // Math Error Protection.
+}
+
+float T1 (int i, Data *DataLine)
+{
+  float x = DataLine->ServoData[i].x;
+  float y = DataLine->ServoData[i].y;
+  float z = DataLine->ServoData[i].z;
+  float x2 = x * x;
+  float y2 = y * y;
+  float z2 = z * z;
+  float l32 = DataLine->l3 * DataLine->l3;
+
+  DataLine->s = sqrt(x2 + y2 + z2);
+
+  float t1 = 0;
+
+  float s2 = DataLine->s * DataLine->s;
+
+  if ((y == 0) && (DataLine->l3 == 0))
+    t1 = 0; // Math Error Protection.
+  else
+    t1 = (atan2(float(DataLine->l3), float(y)));
+
+  float a = (l1 * l1) - (l2*l2) + s2;
+  float b = 2 * l1 * DataLine->s;
+  float t2 = acos(a / b);
+
+  float ret = 0;
+
+  ret = t1 + abs(t2);
+
+  return ret;
+}
+
+float T3 (Data *DataLine)
+{
+  return (acos(((l1*l1) + (l2*l2) - (DataLine->s*DataLine->s)) / (2 * l1 * l2)));
+}
+
+void setleg (int i, Data *DataLine)
+{
+  Segment Seg;
+
+  if (i < 3)
+  {
+    float a = ((float)90 - degrees(O1(i, DataLine)));
+    float b = ((float)180 - degrees(T1(i, DataLine)));
+    float c = degrees(T3(DataLine));
+
+    if ((a >= 0) & (a <= 180)
+      & (b >= 0) & (b <= 180)
+      & (c >= 0) & (c <= 180))
+    {
+      DataLine->ServoData[i].o1 = a;
+      DataLine->ServoData[i].o2 = b;
+      DataLine->ServoData[i].o3 = c;
+    }
+    else
+    {
+      error = true;
+      Seg.sevenSegWrite('e');
+    }
+  }
+  else
+  {
+    float a = ((float)90 + degrees(O1(i, DataLine)));
+    float b = degrees(T1(i, DataLine));
+    float c = ((float)180 - degrees(T3(DataLine)));
+
+    if ((a >= 0) & (a <= 180)
+      & (b >= 0) & (b <= 180)
+      & (c >= 0) & (c <= 180))
+    {
+      DataLine->ServoData[i].o1 = a;
+      DataLine->ServoData[i].o2 = b;
+      DataLine->ServoData[i].o3 = c;
+    }
+    else
+    {
+      error = true;
+      Seg.sevenSegWrite('e');
+    }
+  }
+}
+
+void InverseKinemetics::IK(Data *DataLine)  
+{
+  Segment Seg;
+  Seg.sevenSegWrite('o');
+
+  error = false;
+
+  for (int i=0; i<6; i++)
+    setleg(i, DataLine);
+
+#ifdef DEBUGIK
+  for (int i=0; i<6; i++)
+  {
+    if (i != 0)
+      Serial.print(", ");
+    else
+      Serial.print("### InverseKinemetics->ServosPosition's are: ");
+
+    Serial.print(DataLine->ServoData[i].x);
+    Serial.print(", ");
+    Serial.print(DataLine->ServoData[i].y);
+    Serial.print(", ");
+    Serial.print(DataLine->ServoData[i].z);
+  }
+  if (error == true)
+    Serial.println("; ERROR");
+  else
+    Serial.println(";");
+#endif
+}
+
+