File content as of revision 2:0a7a3c0c08d3:

#include "mbed.h"
#include <math.h>
#include <cmath>
#include "MODSERIAL.h"
#define PI 3.14159265

MODSERIAL pc(USBTX, USBRX);
DigitalIn button1(SW3);
DigitalIn button2(SW2);
DigitalOut LED(LED1);

//Joe dit zijn de inputsignalen
double theta1; double theta4; double emg1; double emg2; double emg3; 
// Joe dit zijn de outputsignalen
double x; double y; 

//Joe dit zijn de constantes 
double ll = 200.0; double lu = 170.0; double lb = 10.0; double le = 79.0; double xbase = 340;
//Joe dit zijn de Tickers

//forward kinematics, Check mathematica! Omdat mbed in paniek raakt met meerdere wortels, hebben we de vergelijking opgedeeld in 3 stukken
//First define the position equation of x
double xendsum = lb + xbase +ll*(cos(theta1) - cos(theta4));
double xendsqrt1 = 2*sqrt(xbase*xbase/4 + lu*lu + ll*(xbase*(cos(theta1)+cos(theta4))/2) -ll*(1+ cos(theta1+theta4)))*(-sin(theta1)+sin(theta4));
double xendsqrt2 = sqrt(pow((-xbase/ll+cos(theta1)+cos(theta4)),2)+ pow(sin(theta1) - sin(theta4),2));
double xend = (xendsum + xendsqrt1/xendsqrt2)/2;
//Now define the pos. eq. of y
double yendsum = le;
double yendsqrt1 = (-xbase/ll + cos(theta1)+cos(theta4))*sqrt(-xbase*xbase/4 + lu*lu + ll/2*(xbase*(cos(theta1)+cos(theta4))- ll*(1+cos(theta1+theta4)))); 
double yendsqrt2 = ll/5*(sin(theta1)+sin(theta4))+ sqrt(pow((-xbase/ll + cos(theta1)+ cos(theta4)),2)+ pow((sin(theta1)-sin(theta4)),2));
float yend = (xendsum + xendsqrt1/xendsqrt2);


//Hier definieren we de functies
Ticker emgcheck;


//Joe, hieronder staan de functies die door de tickers aangeroepen worden
void xcor(){
                            //als emg1==voorbij treshold, 
    theta1 = PI*0.33;        //double theta1-> plus een paar counts (emg*richting)
    theta4 = PI*0.5;          //double theta4-> plus een paar counts (emg*richting)
    xend = (xendsum + xendsqrt1/xendsqrt2)/2;
    //default = als x = xbase/2... break
    //end
    }
void ycor(){
    //als emg2 == voorbij treshold,
    //double theta1 -> plus counts (emg*richting)
    //double theta4 -> plus counts (emg*richting)
    //reken y door
    //default = als y = default... break
    //end
    }
void flip(){}

int main()
{
    
    pc.baud(115200);
    pc.printf("%f\n", xend);
    //default = theta1 = theta4 = pi/2 
    emgcheck.attach(xcor, 0.001);
    emgcheck.attach(ycor, 0.001);
    emgcheck.attach(flip, 0.001);
    
    
    
    
}