File content as of revision 0:1acd9371fe29:

#include "mbed.h"

// >>>>> FUNCTION DEFINITION <<<<<
// Input van deze library is de huidige (x,y)-positie van de pod. Hierdoor wordt een 
// reeks hoeken verkregen waardoor de robot een schietbeweging in y-richting uitvoerd. 
// Voor verdere informatie: zie verslag of MATLAB script.






// temp NOG IN TE VOEREN!!!!!
double x = 1;
double y = 1;
const double y_punch = 0.473;







// >>>>> INITIALISTIONS <<<<<
const double bar1 = 0.260; // length bar 1 [m]
const double bar2 = 0.342; // length bar 2 [m]
const double base_spacing = 0.10559; // spacing between the DC-motors [m]
const double pi = 3.1415926535897;

int main()
{
double virt_bar_left = sqrt(pow(x+0.5*base_spacing,2) + pow(y,2));
double virt_bar_right = sqrt(pow(x-0.5*base_spacing,2) + pow(y,2));
double virt_bar_ref = sqrt(pow(bar2,2)- pow(bar1,2));

// >>>>> LEFT ARM <<<<<
double d2_square = pow(bar1,2) - pow((pow(virt_bar_left,2) + pow(bar1,2) - pow(bar2,2))/(-2*virt_bar_left),2);
double d2 = sqrt(d2_square);
double phi;
if (virt_bar_left < virt_bar_ref)
    phi = pi-asin(d2/bar1);
else
    phi = asin(d2/bar1);
    
double theta_l = pi - phi - acos((x + 0.5*base_spacing)/virt_bar_left);

// >>>>> RIGHT ARM <<<<<
d2_square = pow(bar1,2) - pow((pow(virt_bar_right,2) + pow(bar1,2) - pow(bar2,2))/(-2*virt_bar_right),2);
d2 = sqrt(d2_square);
if (virt_bar_right < virt_bar_ref)
    phi = pi-asin(d2/bar1);
else
    phi = asin(d2/bar1);
double theta_r = phi + acos((x - 0.5*base_spacing)/virt_bar_right);
}