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 |