File content as of revision 7:b59b762c537e:

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

MODSERIAL pc(USBTX, USBRX);  // connecting to pc
DigitalIn button1(SW3);      
DigitalIn button2(SW2);     
DigitalOut ledr(LED1);
DigitalOut led2(LED2);

//Joe dit zijn de inputsignalen
double theta1 = PI*0.5; double theta4 = PI*0.5; double emg1; double emg2; double emg3; double thetaflip = 0;
double omega1; double omega4;
// 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;
double xendsqrt1;
double xendsqrt2;
double xend;
//Now define the pos. eq. of y
double yendsum;
double yendsqrt1;
double yendsqrt2;
double yend;


//Hier definieren we de functies
Ticker emgcheck;
Ticker emgcheck2;


//Joe, hieronder staan de functies die door de tickers aangeroepen worden
void forward(){        //dit is de ticker die zegt, als button=0, theta 1 wordt groter. dan worden x en y doorgerekend
    // hieronder moet veranderd worden naar if button1 == 0, x = x+eenbeetje
    //maar daar moet eerst inverse kinematics voor gebeuren.
    if (button1 == 0){                          //als emg1==voorbij treshold, 
        theta1 = PI*(theta1/PI + 0.1);          //double theta1-> plus een paar counts (emg*richting)
                                                //double theta4-> plus een paar counts (emg*richting)
    //default = als x = xbase/2... break, okee dit moet hier niet
    }
    else {theta1 = theta1;}
    xendsum = lb + xbase +ll*(cos(theta1) - cos(theta4));
    xendsqrt1 = 2*sqrt(-xbase*xbase/4 + lu*lu + ll*(xbase*(cos(theta1)+cos(theta4))/2) -ll*(1+ cos(theta1+theta4)))*(-sin(theta1)+sin(theta4));
    xendsqrt2 = sqrt(pow((-xbase/ll+cos(theta1)+cos(theta4)),2)+ pow(sin(theta1) - sin(theta4),2));
    xend = (xendsum + xendsqrt1/xendsqrt2)/2;
    
    yendsum = -le + ll/2*(sin(theta1)+sin(theta4));
    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)))); 
    yendsqrt2 = sqrt(pow((-xbase/ll + cos(theta1)+ cos(theta4)),2)+ pow((sin(theta1)-sin(theta4)),2));
    yend = (yendsum + yendsqrt1/yendsqrt2);
}
void demomode(){}   //Alleen nodig in de DEMOMODE
                   //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(){
    if(button2==0){thetaflip = PI*(thetaflip/PI+0.5);}
}

void inverse(){
    /*
    We willen ergens beginnen
    Dan de functie qi+1 = qi + (jacobian *(qi - qi-1)/deltaT)deltaT
    if
    */
    theta1-1 = PI*0.5
    theta4-1 = PI*0.5

    theta11 = 
    theta14 = 

    theta1+1
    theta4+1
   
}

int main()
{
    
    pc.baud(115200);
    //default = theta1 = theta4 = pi/2 
    emgcheck.attach(forward, 0.1);
    emgcheck2.attach(flip, 0.1);
    while(true){
        if(xend !=xend){ledr = 0;}
        else{ledr=1;}
           
        if(button2==0){pc.printf("Draai! Hij is plat!");}
    
       if (button1 == 0){
     pc.printf("\n\r %f %f \n\r", xend,yend);
       pc.printf(" %f\n\r", theta1);
       wait(1.5);
      }
        }
    
    
    
}