Subversion Repositories shark

Rev

Rev 1215 | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
1215 giacomo 1
#include <kernel/kern.h>
2
#include <math.h>
3
 
4
#include "servo.h"
5
 
6
#define LEG_A 10.0
7
#define LEG_B 10.0
1329 giacomo 8
#define LEG_C 2.0
9
#define LEG_D 3.0
10
#define LEG_CD_2IPO 2*3.603 // 2*sqrt(LEG_C^2+LEG_D^2)
11
#define LEG_CD_ANG 33.690 // arctg(LEG_C/LEG_D) in gradi
1215 giacomo 12
 
1329 giacomo 13
const double c0 = LEG_C * LEG_C;
14
const double c1 = LEG_B * LEG_B;
15
const double c2 = LEG_B * LEG_B - LEG_A * LEG_A;
1215 giacomo 16
const double todeg = 180.0 / PI;
17
 
18
int set_leg_position(double px, double py, double pz)
19
{
20
 
21
  double px2 = px * px;
22
  double py2 = py * py;
23
  double pz2 = pz * pz;
24
 
1329 giacomo 25
  double pxz2 = px2 + pz2;
1215 giacomo 26
 
1329 giacomo 27
  int alfa,beta,gamma;
28
  double alfa1,beta1,alfa2,beta2,gamma1;
29
  double m,dsqrt;
1215 giacomo 30
 
1329 giacomo 31
  double delta_xz = pxz2 - c0;
32
  double s,k,k2,y1,delta_xy;
1215 giacomo 33
 
1329 giacomo 34
  if (delta_xz < 0.0) return -1;
35
 
36
  if (pz >= LEG_C) {
37
    gamma1 = acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2) * todeg;
38
  } else {
39
    gamma1 = -acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2) * todeg;
40
  }
41
 
42
  if (gamma1 < -90.0 || gamma1 > 90.0) return -1;
43
 
44
  m = pxz2 - LEG_CD_2IPO * (sin(gamma+LEG_CD_ANG)+cos(gamma+LEG_CD_ANG));
45
 
46
  s = m + py2;
47
  k = c2 + s;
48
  k2 = k * k;
49
 
50
  delta_xy = py2 * k2 - s * (k2 - 4.0 * m * c1);
51
 
1215 giacomo 52
  if (delta_xy >= 0.0) {
1329 giacomo 53
 
1215 giacomo 54
    dsqrt = sqrt(delta_xy);
55
    y1 = (py * k + dsqrt) / (2.0 * s);
1329 giacomo 56
    beta1 = asin(y1/LEG_B) * todeg;
57
    alfa1 = asin((y1 - py)/LEG_A) * todeg + beta1;
1215 giacomo 58
    y1 = (py * k - dsqrt) / (2.0 * s);
1329 giacomo 59
    beta2 = asin(y1/LEG_B) * todeg;
60
    alfa2 = asin((y1 - py)/LEG_A) * todeg + beta2;
61
 
62
    if ((alfa1 >= 0.0 && alfa1 <= 180.0) && (beta1 >= -90.0 && beta1 <= 90.0)) {
1215 giacomo 63
      alfa = (int)(alfa1 * 3600.0);
64
      beta = (int)(beta1 * 3600.0);
65
      gamma = (int)(gamma1 * 3600.0);
66
      cprintf("Alfa Sec = %d Beta Sec = %d Gamma Sec = %d\n",alfa,beta,gamma);
67
      return 0;
1329 giacomo 68
    } else if ((alfa2 >= 0.0 && alfa2 <= 180.0) && (beta2 >= -90.0 && beta2 <= 90.0)) {
1215 giacomo 69
      alfa = (int)(alfa2 * 3600.0);
70
      beta = (int)(beta2 * 3600.0);
71
      gamma = (int)(gamma1 * 3600.0);
72
      cprintf("Alfa Sec = %d Beta Sec = %d Gamma Sec = %d\n",alfa,beta,gamma);
73
      return 0;
74
    } else {
75
      cprintf("No possible !\n");
76
      return 1;
77
    }
1329 giacomo 78
 
79
  } else
1215 giacomo 80
    return 1;
81
 
1329 giacomo 82
  return 0;
83
 
1215 giacomo 84
}
1329 giacomo 85