BioRobotics / angleandposition

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?

UserRevisionLine numberNew 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 }