Hexapod InverseKinemetics Problem

Other Hardware Robotics
15

I will post the simplified version that you must run on a pc, but the mathematical part is the same.

#include "stdafx.h"
#include <math.h>
#include <iostream>
#include <fstream>

const double pi = 3.14159265358979;

using namespace std;

float s;
float l3;

#define l1 10
#define l2 15


void RL3 (int x, int y, int z)
{
  if ((x == 0) && (z == 0)) 
	  l3 = 0;
  else l3 = hypot(z, x);

  if (x < 0)
	  l3 = l3 * -1;
}

float O1 (int x, int y, int z)
{
	float t = 0;

  RL3(x, y, z);

  if ((x != 0) & (z != 0))
    t = atan((float) z / x);
  else
    t =  atan(0.0); // Math Error Protection.

  return t;
}

float T1 (int x, int y, int z)
{
  float x2 = x * x;
  float y2 = y * y;
  float z2 = z * z;
  float l32 = l3 * l3;

  s = sqrt(x2 + y2 + z2);

  float t1 = 0;

  float s2 = s * s;

  if ((y == 0) && (l3 == 0))
    t1 = 0; // Math Error Protection.
  else
    t1 = (atan2(float(l3), float(y)));

  float a = (l1 * l1) - (l2*l2) + s2;
  float b = 2 * l1 * s;
  float t2 = acos(a / b);

  float ret = 0;

  ret = t1 + abs(t2);

  return ret;
}

float T3 (int x, int y, int z)
{
  return (acos(((l1*l1) + (l2*l2) - (s*s)) / (2 * l1 * l2)));
}


const float r = (180 / pi);
#define l (l1 + l2)


int _tmain(int argc, _TCHAR* argv[])
{
	/*
		Mine!!!!!

	*/
	ofstream myfile;
	ofstream log;

	myfile.open ("c:\\IK.txt");
	log.open("c:\\log.txt");

	for (int z=-l; z<=l; z++){

		myfile << "Line: " << z << endl;

		for (int y=-l; y<=l; y++){

			for (int x=-l; x<=l; x++)
			{
				float r0 = 90  - (O1(x, y, z)	*	r);
				float r1 = 180 - (T1(x, y, z)	*	r);
				float r2 = T3(x, y, z)			*	r;

				
				//cout << "X = " << x << "\tY = " << y << "\tZ = " << z << "\tR0 = " << float(r0) << "\tR1 = " <<  float(r1) << "\tR2 = " <<  float(r2) << endl;
				log << "X = " << x << "\tY = " << y << "\tZ = " << z << "\tR0 = " <<  float(r0) << "\tR1 = " <<  float(r1) << "\tR2 = " <<  float(r2) << endl;

				if (((r0 <= 180) & (r0 >= 0))
					& ((r1 <= 180) & (r1 >= 0))
						& (r2 <= 180) & (r2 >= 0)){
							myfile << "|";
						}
				else
					myfile << "_";

				if (x == 0)
					myfile << " ";
			}

			myfile << endl;

			if (y == 0)
				myfile << endl;
		}

		myfile << endl << endl << endl << endl << endl;
	}

	myfile.close();
	log.close();

	system("c:\\IK.txt");
	//system("c:\\log.txt");

	return 0;
}