File content as of revision 0:779fe292e912:

#include "mbed.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; double lu; double lb; double lend;
//Joe dit zijn de Tickers
Ticker emgcheck

//forward kinematics
double alfax = 1/2 (lb + lix + ll (Cos[theta1] - Cos[theta4]) + (
   2 Sqrt[-(lix^2/4) + lu^2 + 
     1/2 ll (lix (Cos[theta1] + Cos[theta4]) - ll (1 + Cos[theta1 + theta4]))] (-Sin[
        theta1] + Sin[theta4]))/
   Sqrt[(-(lix/ll) + Cos[theta1] + Cos[theta4])^2 + (Sin[theta1] - Sin[theta4)^2]
double alfay = -le + ((-(lix/ll) + Cos[theta1] + Cos[theta4]) Sqrt[-(lix^2/4) + lu^2 + 
   1/2 ll (lix (Cos[theta1] + Cos[theta4]) - ll (1 + Cos[theta1 + theta4]))])/Sqrt[(-(
     lix/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
    //end
    }
void ycor(){}
void flip(){}

int main()
{
    //default = theta1 = theta4 = pi/2 
    emgcheck.attach(xcor, 0.001);
    emgcheck.attach(ycor, 0.001);
    emgcheck.attach(flip, 0.001);
    
    
    
}