Yannick Buser / Mbed 2 deprecated position2angle

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

Dependencies:   mbed

Fork of position2angle by Yannick Buser

main.cpp@0:1acd9371fe29, 2015-10-19 (annotated)

Committer:
ymbuser
Date:
Mon Oct 19 08:59:09 2015 +0000
Revision:
0:1acd9371fe29
Shoot function for a five bar mechanism

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 0:1acd9371fe29 4 // Input van deze library is de huidige (x,y)-positie van de pod. Hierdoor wordt een
ymbuser 0:1acd9371fe29 5 // reeks hoeken verkregen waardoor de robot een schietbeweging in y-richting uitvoerd.
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
ymbuser 0:1acd9371fe29 11
ymbuser 0:1acd9371fe29 12
ymbuser 0:1acd9371fe29 13 // temp NOG IN TE VOEREN!!!!!
ymbuser 0:1acd9371fe29 14 double x = 1;
ymbuser 0:1acd9371fe29 15 double y = 1;
ymbuser 0:1acd9371fe29 16 const double y_punch = 0.473;
ymbuser 0:1acd9371fe29 17
ymbuser 0:1acd9371fe29 18
ymbuser 0:1acd9371fe29 19
ymbuser 0:1acd9371fe29 20
ymbuser 0:1acd9371fe29 21
ymbuser 0:1acd9371fe29 22
ymbuser 0:1acd9371fe29 23
ymbuser 0:1acd9371fe29 24 // >>>>> INITIALISTIONS <<<<<
ymbuser 0:1acd9371fe29 25 const double bar1 = 0.260; // length bar 1 [m]
ymbuser 0:1acd9371fe29 26 const double bar2 = 0.342; // length bar 2 [m]
ymbuser 0:1acd9371fe29 27 const double base_spacing = 0.10559; // spacing between the DC-motors [m]
ymbuser 0:1acd9371fe29 28 const double pi = 3.1415926535897;
ymbuser 0:1acd9371fe29 29
ymbuser 0:1acd9371fe29 30 int main()
ymbuser 0:1acd9371fe29 31 {
ymbuser 0:1acd9371fe29 32 double virt_bar_left = sqrt(pow(x+0.5*base_spacing,2) + pow(y,2));
ymbuser 0:1acd9371fe29 33 double virt_bar_right = sqrt(pow(x-0.5*base_spacing,2) + pow(y,2));
ymbuser 0:1acd9371fe29 34 double virt_bar_ref = sqrt(pow(bar2,2)- pow(bar1,2));
ymbuser 0:1acd9371fe29 35
ymbuser 0:1acd9371fe29 36 // >>>>> LEFT ARM <<<<<
ymbuser 0:1acd9371fe29 37 double d2_square = pow(bar1,2) - pow((pow(virt_bar_left,2) + pow(bar1,2) - pow(bar2,2))/(-2*virt_bar_left),2);
ymbuser 0:1acd9371fe29 38 double d2 = sqrt(d2_square);
ymbuser 0:1acd9371fe29 39 double phi;
ymbuser 0:1acd9371fe29 40 if (virt_bar_left < virt_bar_ref)
ymbuser 0:1acd9371fe29 41 phi = pi-asin(d2/bar1);
ymbuser 0:1acd9371fe29 42 else
ymbuser 0:1acd9371fe29 43 phi = asin(d2/bar1);
ymbuser 0:1acd9371fe29 44
ymbuser 0:1acd9371fe29 45 double theta_l = pi - phi - acos((x + 0.5*base_spacing)/virt_bar_left);
ymbuser 0:1acd9371fe29 46
ymbuser 0:1acd9371fe29 47 // >>>>> RIGHT ARM <<<<<
ymbuser 0:1acd9371fe29 48 d2_square = pow(bar1,2) - pow((pow(virt_bar_right,2) + pow(bar1,2) - pow(bar2,2))/(-2*virt_bar_right),2);
ymbuser 0:1acd9371fe29 49 d2 = sqrt(d2_square);
ymbuser 0:1acd9371fe29 50 if (virt_bar_right < virt_bar_ref)
ymbuser 0:1acd9371fe29 51 phi = pi-asin(d2/bar1);
ymbuser 0:1acd9371fe29 52 else
ymbuser 0:1acd9371fe29 53 phi = asin(d2/bar1);
ymbuser 0:1acd9371fe29 54 double theta_r = phi + acos((x - 0.5*base_spacing)/virt_bar_right);
ymbuser 0:1acd9371fe29 55 }