Library to calculate angles from positions and vice versa (also used for shooting angles)
Dependents: includeair includeair calcul_Ueff2 Mesure_energie
angleandposition.cpp@5:2dc21bf3f3a0, 2015-10-29 (annotated)
- Committer:
- Gerth
- Date:
- Thu Oct 29 14:51:02 2015 +0000
- Revision:
- 5:2dc21bf3f3a0
- Parent:
- 4:9492c1c361b9
corrected for deviation of angle
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Gerth | 0:b8295c4b5793 | 1 | #include "angleandposition.h" |
Gerth | 0:b8295c4b5793 | 2 | #include "mbed.h" |
Gerth | 0:b8295c4b5793 | 3 | #include "math.h" |
Gerth | 0:b8295c4b5793 | 4 | |
Gerth | 5:2dc21bf3f3a0 | 5 | |
Gerth | 5:2dc21bf3f3a0 | 6 | const double correctionforupperarm= acos((0.255/0.260)); |
Gerth | 2:b99fef8df97e | 7 | const double bar1 = 0.260; // length bar 1 [mm] |
Gerth | 2:b99fef8df97e | 8 | const double bar2 = 0.342; // length bar 2 [mm] |
Gerth | 2:b99fef8df97e | 9 | const double base_spacing = 0.10559; // spacing between the DC-motors [mm] |
Gerth | 1:6cace9fdb088 | 10 | const double pi = 3.1415926535897; |
Gerth | 1:6cace9fdb088 | 11 | |
Gerth | 5:2dc21bf3f3a0 | 12 | const double virt_bar_ref = sqrt(pow(bar2,2)- pow(bar1,2)); |
Gerth | 0:b8295c4b5793 | 13 | |
Gerth | 0:b8295c4b5793 | 14 | |
Gerth | 0:b8295c4b5793 | 15 | angleandposition::angleandposition(void) |
Gerth | 0:b8295c4b5793 | 16 | { |
Gerth | 1:6cace9fdb088 | 17 | |
Gerth | 1:6cace9fdb088 | 18 | } |
Gerth | 1:6cace9fdb088 | 19 | |
Gerth | 4:9492c1c361b9 | 20 | double angleandposition::positiontoangle1(double x_position, double y_position)//rigth arm |
Gerth | 5:2dc21bf3f3a0 | 21 | { |
Gerth | 1:6cace9fdb088 | 22 | double virt_bar_right = sqrt( pow(x_position-0.5*base_spacing,2) + pow(y_position,2) ); |
Gerth | 1:6cace9fdb088 | 23 | |
Gerth | 1:6cace9fdb088 | 24 | double d2_square = pow(bar1,2) - pow( ( pow(virt_bar_right,2) + pow(bar1,2) - pow(bar2,2) ) / (-2*virt_bar_right) ,2); |
Gerth | 0:b8295c4b5793 | 25 | |
Gerth | 1:6cace9fdb088 | 26 | double d2 = sqrt(d2_square); |
Gerth | 3:067c75c62882 | 27 | |
Gerth | 1:6cace9fdb088 | 28 | double phi; |
Gerth | 1:6cace9fdb088 | 29 | if (virt_bar_right <= virt_bar_ref) { |
Gerth | 3:067c75c62882 | 30 | phi = pi-asin(d2/bar1); |
Gerth | 1:6cace9fdb088 | 31 | } else { |
Gerth | 2:b99fef8df97e | 32 | phi = asin(d2/bar1); |
Gerth | 0:b8295c4b5793 | 33 | } |
Gerth | 2:b99fef8df97e | 34 | double theta_r = -phi + acos((x_position - 0.5*base_spacing)/virt_bar_right); |
Gerth | 5:2dc21bf3f3a0 | 35 | return theta_r-correctionforupperarm; |
Gerth | 0:b8295c4b5793 | 36 | } |
Gerth | 0:b8295c4b5793 | 37 | |
Gerth | 4:9492c1c361b9 | 38 | double angleandposition::positiontoangle2(double x_position, double y_position)//left arm |
Gerth | 0:b8295c4b5793 | 39 | { |
Gerth | 2:b99fef8df97e | 40 | double virt_bar_left = sqrt(pow((x_position + 0.5*base_spacing) ,2) + pow(y_position,2) ); |
Gerth | 3:067c75c62882 | 41 | |
Gerth | 1:6cace9fdb088 | 42 | double d2_square = pow(bar1,2) - pow( ( pow(virt_bar_left,2) + pow(bar1,2) - pow(bar2,2) ) / (-2*virt_bar_left), 2); |
Gerth | 3:067c75c62882 | 43 | |
Gerth | 1:6cace9fdb088 | 44 | double d2 = sqrt(d2_square); |
Gerth | 3:067c75c62882 | 45 | |
Gerth | 1:6cace9fdb088 | 46 | double phi; |
Gerth | 3:067c75c62882 | 47 | |
Gerth | 1:6cace9fdb088 | 48 | if (virt_bar_left <= virt_bar_ref) { |
Gerth | 3:067c75c62882 | 49 | phi = pi-asin(d2/bar1); |
Gerth | 1:6cace9fdb088 | 50 | } else { |
Gerth | 3:067c75c62882 | 51 | phi = asin(d2/bar1); |
Gerth | 1:6cace9fdb088 | 52 | } |
Gerth | 0:b8295c4b5793 | 53 | |
Gerth | 1:6cace9fdb088 | 54 | double theta_l = pi - phi - acos((x_position + 0.5*base_spacing)/virt_bar_left); |
Gerth | 0:b8295c4b5793 | 55 | |
Gerth | 5:2dc21bf3f3a0 | 56 | return theta_l-correctionforupperarm; |
Gerth | 3:067c75c62882 | 57 | } |
Gerth | 3:067c75c62882 | 58 | |
Gerth | 4:9492c1c361b9 | 59 | double angleandposition::angletoposition(double theta_r,double theta_l) |
Gerth | 3:067c75c62882 | 60 | { |
Gerth | 5:2dc21bf3f3a0 | 61 | theta_r+=correctionforupperarm; |
Gerth | 5:2dc21bf3f3a0 | 62 | theta_l+=correctionforupperarm; |
Gerth | 3:067c75c62882 | 63 | |
Gerth | 3:067c75c62882 | 64 | double virt_bar_x = base_spacing + cos(theta_l)*bar1 + cos(theta_r)*bar1; |
Gerth | 3:067c75c62882 | 65 | double virt_bar_y = sin(theta_l)*bar1 - sin(theta_r)*bar1; |
Gerth | 3:067c75c62882 | 66 | double virt_bar = sqrt(pow(virt_bar_x,2) + pow(virt_bar_y,2)); |
Gerth | 3:067c75c62882 | 67 | |
Gerth | 3:067c75c62882 | 68 | |
Gerth | 3:067c75c62882 | 69 | double phi = acos(0.5*virt_bar/bar2); |
Gerth | 3:067c75c62882 | 70 | double phi_diff = atan(abs(virt_bar_y)/virt_bar_x); |
Gerth | 3:067c75c62882 | 71 | |
Gerth | 3:067c75c62882 | 72 | double theta_l2; |
Gerth | 3:067c75c62882 | 73 | if (sin(theta_l) >= sin(theta_r)) { |
Gerth | 3:067c75c62882 | 74 | theta_l2 = phi - phi_diff; |
Gerth | 3:067c75c62882 | 75 | } else { |
Gerth | 3:067c75c62882 | 76 | theta_l2 = phi + phi_diff; |
Gerth | 3:067c75c62882 | 77 | } |
Gerth | 3:067c75c62882 | 78 | |
Gerth | 3:067c75c62882 | 79 | double x = -cos(theta_l)*bar1 - 0.5*base_spacing + cos(theta_l2)*bar2; |
Gerth | 3:067c75c62882 | 80 | |
Gerth | 3:067c75c62882 | 81 | return x; |
Gerth | 3:067c75c62882 | 82 | } |
Gerth | 3:067c75c62882 | 83 | |
Gerth | 4:9492c1c361b9 | 84 | double angleandposition::angletoposition_y (double theta_r, double theta_l) |
Gerth | 3:067c75c62882 | 85 | { |
Gerth | 5:2dc21bf3f3a0 | 86 | theta_r+=correctionforupperarm; |
Gerth | 5:2dc21bf3f3a0 | 87 | theta_l+=correctionforupperarm; |
Gerth | 5:2dc21bf3f3a0 | 88 | |
Gerth | 3:067c75c62882 | 89 | double virt_bar_x = base_spacing + cos(theta_l)*bar1 + cos(theta_r)*bar1; |
Gerth | 3:067c75c62882 | 90 | double virt_bar_y = sin(theta_l)*bar1 - sin(theta_r)*bar1; |
Gerth | 3:067c75c62882 | 91 | double virt_bar = sqrt(pow(virt_bar_x,2) + pow(virt_bar_y,2)); |
Gerth | 3:067c75c62882 | 92 | |
Gerth | 3:067c75c62882 | 93 | double phi = acos(0.5*virt_bar/bar2); |
Gerth | 3:067c75c62882 | 94 | double phi_diff = atan(abs(virt_bar_y)/virt_bar_x); |
Gerth | 3:067c75c62882 | 95 | |
Gerth | 3:067c75c62882 | 96 | double theta_l2; |
Gerth | 3:067c75c62882 | 97 | if (sin(theta_l) >= sin(theta_r)) { |
Gerth | 3:067c75c62882 | 98 | theta_l2 = phi - phi_diff; |
Gerth | 3:067c75c62882 | 99 | } else { |
Gerth | 3:067c75c62882 | 100 | theta_l2 = phi + phi_diff; |
Gerth | 3:067c75c62882 | 101 | } |
Gerth | 3:067c75c62882 | 102 | double y = sin(theta_l)*bar1 + sin(theta_l2)*bar2; |
Gerth | 3:067c75c62882 | 103 | |
Gerth | 3:067c75c62882 | 104 | return y; |
Gerth | 0:b8295c4b5793 | 105 | } |