Subversion Repositories shark

#include <kernel/kern.h>
#include <math.h>

#include "servo.h"

#define LEG_A 10.0
#define LEG_B 10.0

const double c0 = LEG_A * LEG_A;
const double c1 = LEG_A * LEG_A - LEG_B * LEG_B;
const double todeg = 180.0 / PI;

int set_leg_position(double px, double py, double pz)
{

  double px2 = px * px;
  double py2 = py * py;
  double pz2 = pz * pz;

  double pxz2 = pz2 + px2;
  double gamma1 = asin(pz/sqrt(pxz2)) * todeg;

  double s = pxz2 + py2;
  double k = c1 + s;
  double k2 = k * k;

  double delta_xy = py2 * k2 - s * (k2 - 4.0 * pxz2 * c0);
  double y1,dsqrt;
  double alfa1,beta1,alfa2,beta2;
  int alfa = 0, beta = 0, gamma = 0;

  if (delta_xy >= 0.0) {
    dsqrt = sqrt(delta_xy);
    y1 = (py * k + dsqrt) / (2.0 * s);
    alfa1 = asin(y1/LEG_A) * todeg;
    beta1 = asin((y1 - py)/LEG_B) * todeg + alfa1;
    y1 = (py * k - dsqrt) / (2.0 * s);
    alfa2 = asin(y1/LEG_A) * todeg;
    beta2 = asin((y1 - py)/LEG_B) * todeg + alfa2;
    cprintf("Alfa1 = %d Beta1 = %d\n",(int)alfa1,(int)beta1);
    cprintf("Alfa2 = %d Beta2 = %d\n",(int)alfa2,(int)beta2);
    if ((alfa1 >= -90.0 && alfa1 <= 90.0) && (beta1 >= 0.0 && beta1 <= 180.0)) {
      alfa = (int)(alfa1 * 3600.0);
      beta = (int)(beta1 * 3600.0);
      gamma = (int)(gamma1 * 3600.0);
      cprintf("Alfa Sec = %d Beta Sec = %d Gamma Sec = %d\n",alfa,beta,gamma);
      return 0;
    } else if ((alfa2 >= -90.0 && alfa2 <= 90.0) && (beta2 >= 0.0 && beta2 <= 180.0)) {
      alfa = (int)(alfa2 * 3600.0);
      beta = (int)(beta2 * 3600.0);
      gamma = (int)(gamma1 * 3600.0);
      cprintf("Alfa Sec = %d Beta Sec = %d Gamma Sec = %d\n",alfa,beta,gamma);
      return 0;
    } else {
      cprintf("No possible !\n");
      return 1;
    }
  } else {
    cprintf("No possible !\n");
    return 1;
  }

}