Yannick Buser / Mbed 2 deprecated angle2position

Algorithm for a five bar mechanism which transfers set of angles of the DC motors to the position of the end effector.

Dependencies:   mbed

main.cpp@1:8bc06db8386e, 2015-10-20 (annotated)

Committer:
ymbuser
Date:
Tue Oct 20 12:57:58 2015 +0000
Revision:
1:8bc06db8386e
Parent:
0:1acd9371fe29 
Algorithm for a five bar mechanism which transfers  a set of angles for the DC motors to the position of the end effector.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ymbuser 0:1acd9371fe29 1 #include "mbed.h"
ymbuser 0:1acd9371fe29 2
ymbuser 0:1acd9371fe29 3 // >>>>> FUNCTION DEFINITION <<<<<
ymbuser 1:8bc06db8386e 4 // Input van deze library zijn de hoeken van de motoren. Hierdoor wordt een
ymbuser 1:8bc06db8386e 5 // 'real time' positie verkregen waardoor deze waarden als input gegeven kunnen worden.
ymbuser 0:1acd9371fe29 6 // Voor verdere informatie: zie verslag of MATLAB script.
ymbuser 0:1acd9371fe29 7
ymbuser 0:1acd9371fe29 8
ymbuser 0:1acd9371fe29 9
ymbuser 0:1acd9371fe29 10 // temp NOG IN TE VOEREN!!!!!
ymbuser 1:8bc06db8386e 11 double theta_l = 0;
ymbuser 1:8bc06db8386e 12 double theta_r = 0;
ymbuser 0:1acd9371fe29 13
ymbuser 0:1acd9371fe29 14
ymbuser 0:1acd9371fe29 15
ymbuser 0:1acd9371fe29 16 // >>>>> INITIALISTIONS <<<<<
ymbuser 0:1acd9371fe29 17 const double bar1 = 0.260; // length bar 1 [m]
ymbuser 0:1acd9371fe29 18 const double bar2 = 0.342; // length bar 2 [m]
ymbuser 0:1acd9371fe29 19 const double base_spacing = 0.10559; // spacing between the DC-motors [m]
ymbuser 0:1acd9371fe29 20
ymbuser 0:1acd9371fe29 21 int main()
ymbuser 0:1acd9371fe29 22 {
ymbuser 1:8bc06db8386e 23 double virt_bar_x = base_spacing + cos(theta_l)*bar1 + cos(theta_r)*bar1;
ymbuser 1:8bc06db8386e 24 double virt_bar_y = sin(theta_l)*bar1 - sin(theta_r)*bar1;
ymbuser 1:8bc06db8386e 25 double virt_bar = sqrt(pow(virt_bar_x,2) + pow(virt_bar_y,2));
ymbuser 1:8bc06db8386e 26 // virt_bar = virtuele staaf tussen de twee scharnieren van de arm
ymbuser 1:8bc06db8386e 27
ymbuser 1:8bc06db8386e 28 double phi = acos(0.5*virt_bar/bar2);
ymbuser 1:8bc06db8386e 29 double phi_diff = atan(abs(virt_bar_y)/virt_bar_x);
ymbuser 0:1acd9371fe29 30
ymbuser 1:8bc06db8386e 31 double theta_l2, theta_r2;
ymbuser 1:8bc06db8386e 32 if (sin(theta_l) >= sin(theta_r)) {
ymbuser 1:8bc06db8386e 33 theta_l2 = phi - phi_diff;
ymbuser 1:8bc06db8386e 34 theta_r2 = phi + phi_diff;
ymbuser 1:8bc06db8386e 35 } else {
ymbuser 1:8bc06db8386e 36 theta_l2 = phi + phi_diff;
ymbuser 1:8bc06db8386e 37 theta_r2 = phi - phi_diff;
ymbuser 1:8bc06db8386e 38 }
ymbuser 0:1acd9371fe29 39
ymbuser 1:8bc06db8386e 40 double x = -cos(theta_l)*bar1 - 0.5*base_spacing + cos(theta_l2)*bar2;
ymbuser 1:8bc06db8386e 41 double y = sin(theta_l)*bar1 + sin(theta_l2)*bar2;
ymbuser 0:1acd9371fe29 42 }