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@4:9492c1c361b9, 2015-10-29 (annotated)

Committer:
Gerth
Date:
Thu Oct 29 14:03:25 2015 +0000
Revision:
4:9492c1c361b9
Parent:
3:067c75c62882
Child:
5:2dc21bf3f3a0
converted floats to doubles

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 2:b99fef8df97e 5 const double bar1 = 0.260; // length bar 1 [mm]
Gerth 2:b99fef8df97e 6 const double bar2 = 0.342; // length bar 2 [mm]
Gerth 2:b99fef8df97e 7 const double base_spacing = 0.10559; // spacing between the DC-motors [mm]
Gerth 1:6cace9fdb088 8 const double pi = 3.1415926535897;
Gerth 1:6cace9fdb088 9
Gerth 1:6cace9fdb088 10 double virt_bar_ref = sqrt(pow(bar2,2)- pow(bar1,2));
Gerth 0:b8295c4b5793 11
Gerth 0:b8295c4b5793 12
Gerth 0:b8295c4b5793 13 angleandposition::angleandposition(void)
Gerth 0:b8295c4b5793 14 {
Gerth 1:6cace9fdb088 15
Gerth 1:6cace9fdb088 16 }
Gerth 1:6cace9fdb088 17
Gerth 4:9492c1c361b9 18 double angleandposition::positiontoangle1(double x_position, double y_position)//rigth arm
Gerth 1:6cace9fdb088 19 {
Gerth 1:6cace9fdb088 20 double virt_bar_right = sqrt( pow(x_position-0.5*base_spacing,2) + pow(y_position,2) );
Gerth 1:6cace9fdb088 21
Gerth 1:6cace9fdb088 22 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 23
Gerth 1:6cace9fdb088 24 double d2 = sqrt(d2_square);
Gerth 3:067c75c62882 25
Gerth 1:6cace9fdb088 26 double phi;
Gerth 1:6cace9fdb088 27 if (virt_bar_right <= virt_bar_ref) {
Gerth 3:067c75c62882 28 phi = pi-asin(d2/bar1);
Gerth 1:6cace9fdb088 29 } else {
Gerth 2:b99fef8df97e 30 phi = asin(d2/bar1);
Gerth 0:b8295c4b5793 31 }
Gerth 2:b99fef8df97e 32 double theta_r = -phi + acos((x_position - 0.5*base_spacing)/virt_bar_right);
Gerth 1:6cace9fdb088 33 return theta_r;
Gerth 0:b8295c4b5793 34 }
Gerth 0:b8295c4b5793 35
Gerth 4:9492c1c361b9 36 double angleandposition::positiontoangle2(double x_position, double y_position)//left arm
Gerth 0:b8295c4b5793 37 {
Gerth 2:b99fef8df97e 38 double virt_bar_left = sqrt(pow((x_position + 0.5*base_spacing) ,2) + pow(y_position,2) );
Gerth 3:067c75c62882 39
Gerth 1:6cace9fdb088 40 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 41
Gerth 1:6cace9fdb088 42 double d2 = sqrt(d2_square);
Gerth 3:067c75c62882 43
Gerth 1:6cace9fdb088 44 double phi;
Gerth 3:067c75c62882 45
Gerth 1:6cace9fdb088 46 if (virt_bar_left <= virt_bar_ref) {
Gerth 3:067c75c62882 47 phi = pi-asin(d2/bar1);
Gerth 1:6cace9fdb088 48 } else {
Gerth 3:067c75c62882 49 phi = asin(d2/bar1);
Gerth 1:6cace9fdb088 50 }
Gerth 0:b8295c4b5793 51
Gerth 1:6cace9fdb088 52 double theta_l = pi - phi - acos((x_position + 0.5*base_spacing)/virt_bar_left);
Gerth 0:b8295c4b5793 53
Gerth 1:6cace9fdb088 54 return theta_l;
Gerth 3:067c75c62882 55 }
Gerth 3:067c75c62882 56
Gerth 4:9492c1c361b9 57 double angleandposition::angletoposition(double theta_r,double theta_l)
Gerth 3:067c75c62882 58 {
Gerth 3:067c75c62882 59
Gerth 3:067c75c62882 60 double virt_bar_x = base_spacing + cos(theta_l)*bar1 + cos(theta_r)*bar1;
Gerth 3:067c75c62882 61 double virt_bar_y = sin(theta_l)*bar1 - sin(theta_r)*bar1;
Gerth 3:067c75c62882 62 double virt_bar = sqrt(pow(virt_bar_x,2) + pow(virt_bar_y,2));
Gerth 3:067c75c62882 63
Gerth 3:067c75c62882 64
Gerth 3:067c75c62882 65 double phi = acos(0.5*virt_bar/bar2);
Gerth 3:067c75c62882 66 double phi_diff = atan(abs(virt_bar_y)/virt_bar_x);
Gerth 3:067c75c62882 67
Gerth 3:067c75c62882 68 double theta_l2;
Gerth 3:067c75c62882 69 if (sin(theta_l) >= sin(theta_r)) {
Gerth 3:067c75c62882 70 theta_l2 = phi - phi_diff;
Gerth 3:067c75c62882 71 } else {
Gerth 3:067c75c62882 72 theta_l2 = phi + phi_diff;
Gerth 3:067c75c62882 73 }
Gerth 3:067c75c62882 74
Gerth 3:067c75c62882 75 double x = -cos(theta_l)*bar1 - 0.5*base_spacing + cos(theta_l2)*bar2;
Gerth 3:067c75c62882 76
Gerth 3:067c75c62882 77 return x;
Gerth 3:067c75c62882 78 }
Gerth 3:067c75c62882 79
Gerth 4:9492c1c361b9 80 double angleandposition::angletoposition_y (double theta_r, double theta_l)
Gerth 3:067c75c62882 81 {
Gerth 3:067c75c62882 82 double virt_bar_x = base_spacing + cos(theta_l)*bar1 + cos(theta_r)*bar1;
Gerth 3:067c75c62882 83 double virt_bar_y = sin(theta_l)*bar1 - sin(theta_r)*bar1;
Gerth 3:067c75c62882 84 double virt_bar = sqrt(pow(virt_bar_x,2) + pow(virt_bar_y,2));
Gerth 3:067c75c62882 85
Gerth 3:067c75c62882 86 double phi = acos(0.5*virt_bar/bar2);
Gerth 3:067c75c62882 87 double phi_diff = atan(abs(virt_bar_y)/virt_bar_x);
Gerth 3:067c75c62882 88
Gerth 3:067c75c62882 89 double theta_l2;
Gerth 3:067c75c62882 90 if (sin(theta_l) >= sin(theta_r)) {
Gerth 3:067c75c62882 91 theta_l2 = phi - phi_diff;
Gerth 3:067c75c62882 92 } else {
Gerth 3:067c75c62882 93 theta_l2 = phi + phi_diff;
Gerth 3:067c75c62882 94 }
Gerth 3:067c75c62882 95 double y = sin(theta_l)*bar1 + sin(theta_l2)*bar2;
Gerth 3:067c75c62882 96
Gerth 3:067c75c62882 97 return y;
Gerth 0:b8295c4b5793 98 }