mangavalk
413f52c008
GPS include disabled, out-of-reach flag instead of the 7-segment error. Exact date unknown; dated at the archive's first commit (Feb 2014).
153 lines
3.0 KiB
C++
153 lines
3.0 KiB
C++
#include "InverseKinemetics.h"
|
|
// V3.0
|
|
|
|
//Variables
|
|
boolean error;
|
|
boolean OOF; // OutOfReach
|
|
|
|
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
|
|
{
|
|
OOF = 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
|
|
{
|
|
OOF = true;
|
|
//Seg.sevenSegWrite('e');
|
|
}
|
|
}
|
|
}
|
|
|
|
void InverseKinemetics::IK(Data *DataLine)
|
|
{
|
|
//Segment Seg;
|
|
//Seg.sevenSegWrite('o');
|
|
|
|
error = false;
|
|
OOF = 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->ServoData[" + i + "]: ");
|
|
|
|
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
|
|
}
|
|
|
|
|