File content as of revision 1:8bc06db8386e:

#include "mbed.h"

// >>>>> FUNCTION DEFINITION <<<<<
// Input van deze library zijn de hoeken van de motoren. Hierdoor wordt een
// 'real time' positie verkregen waardoor deze waarden als input gegeven kunnen worden.
// Voor verdere informatie: zie verslag of MATLAB script.



// temp NOG IN TE VOEREN!!!!!
double theta_l = 0;
double theta_r = 0;



// >>>>> 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]

int main()
{
    double virt_bar_x = base_spacing + cos(theta_l)*bar1 + cos(theta_r)*bar1;
    double virt_bar_y = sin(theta_l)*bar1 - sin(theta_r)*bar1;
    double virt_bar = sqrt(pow(virt_bar_x,2) + pow(virt_bar_y,2));
// virt_bar = virtuele staaf tussen de twee scharnieren van de arm

    double phi = acos(0.5*virt_bar/bar2);
    double phi_diff = atan(abs(virt_bar_y)/virt_bar_x);

    double theta_l2, theta_r2;
    if (sin(theta_l) >= sin(theta_r)) {
        theta_l2 = phi - phi_diff;
        theta_r2 = phi + phi_diff;
    } else {
        theta_l2 = phi + phi_diff;
        theta_r2 = phi - phi_diff;
    }

    double x = -cos(theta_l)*bar1 - 0.5*base_spacing + cos(theta_l2)*bar2;
    double y = sin(theta_l)*bar1 + sin(theta_l2)*bar2;
}