File content as of revision 2:e2af01393e16:

#include "mbed.h"
#include "math.h"
#include "HIDScope.h"

HIDScope scope(4);
AnalogIn    potright(A0);
AnalogIn    potleft(A1);
// x & y zijn inputs L is lengte van de arm r is de vector naar de end effector

const double L = 36;
const double pi = 3.1415926535897;
double theta1;
double theta2;

// functies die de hoeken berekend
void getangles(double &theta_one,double &theta_two) // xy inputs
{
    double x = 60+ 20*potleft.read();
    double y = 40+40*potright.read();
    double r = sqrt(pow(L,2)+pow(L,2)); // vector naar end effector
    double alfa = acos((2*pow(L,2)-pow(r,2))/(4*L)); // alfa is de hoek tussen upper en lower arm
    double beta = acos((pow(r,2))/(2*L*r)); // beta is de hoek tussen upper arm en r
    // hoeken berekenen
    theta_one = atan2(y,x)+beta;
    theta_two = pi + alfa;
    scope.set(0,x);
    scope.set(1,y);
    double xt = L*cos(theta_one)+L*cos(theta_one+theta_two);
    double yt = L*cos(theta_one)+L*cos(theta_one+theta_two);
    scope.set(2,xt);
    scope.set(3,yt);
    scope.send();
}

int main()
{
    while(true) {
        getangles(theta1,theta2);
    }
}