File content as of revision 1:f63be2020475:

#include "mbed.h"
#include "math.h"
#define PI 3.14159265



//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 = 350.0-lb;
//Joe dit zijn de Tickers
Ticker emgcheck;


//forward kinematics, Check mathematica!
double alfax = 1.0/2.0 *(lb + xbase + ll*(cos(theta1) - cos(theta4)) + (2.0*pow(-(pow(xbase, 2.0))/4 + lu^2 + 1/2*ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))),0.5)*
(-sin(theta1) + sin(theta4)))/sqrt((-(xbase/ll) + cos(theta1) + cos(theta4))^2 + (sin(theta1) - sin(theta4)^2)
double alfay = -le + ((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-(xbase^2/4) + lu^2 + 
   1/2 ll (xbase (cos(theta1) + cos(theta4)) - ll (1 + cos(theta1 + theta4)))))/ sqrt((-(
     xbase/ll) + cos(theta1) + cos(theta4))^2 + (sin(theta1) - sin(theta4))^2) + 
 1/2 ll (sin(theta1) + sin(theta4));

//Joe, hieronder staan de functies die door de tickers aangeroepen worden
void xcor(){
    //als emg1==voorbij treshold, 
    //double theta1-> plus een paar counts (emg*richting)
    //double theta4-> plus een paar counts (emg*richting)
    //reken x door
    //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()
{
    //default = theta1 = theta4 = pi/2 
    emgcheck.attach(xcor, 0.001);
    emgcheck.attach(ycor, 0.001);
    emgcheck.attach(flip, 0.001);
    
    
    
}