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@3:9ab195fa532b, 2015-10-27 (annotated)

Committer:
ymbuser
Date:
Tue Oct 27 15:52:40 2015 +0000
Revision:
3:9ab195fa532b
Algorithm which transfers the angle positions of the DC motors to the position of the pod;

Who changed what in which revision?

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