File content as of revision 11:256e84fea925:

#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);      // defining testbutton NEEDS TO BE REMOVED
DigitalOut ledr(LED1);      // Only for testing 
//DigitalOut led2(LED2);      // Only for testing
InterruptIn button2(SW2);   //Only for testing

// nog te verwijderen/ aan te passen, zijn dubbel gedefinieerd
double theta11;

//Joe dit zijn de inputsignalen (en tussenvariabelen)
double theta10;                //vorige theta
double theta1;                  // huidige/nieuwe theta
double theta40;
double theta4; 
bool emg1; 
bool emg2; 
bool emg3; 
double thetaflip = 0;
double omega1; 
double omega4; 
double prefx;
double prefy;
double deltat = 0.01;
//Joe dit zijn de constantes 
double ll = 200.0; 
double lu = 170.0; 
double lb = 10.0; 
double le = 79.0; 
double xbase = 340;


//forward kinematics, Check mathematica! Omdat mbed in paniek raakt met meerdere wortels, hebben we de vergelijking opgedeeld in 3 stukken
//check void forward voor de berekeningen
//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 en tickers
Ticker emgcheck;
Ticker emgcheck2;
Ticker emgcheck3; 


//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, komt waarschijnlijk in de inverse functie
        theta1 = PI*(theta1/PI + 0.1);          
     hij is geblokt omdat ik de knop nodig heb
    //default = als x = xbase/2... break, okee dit moet hier niet, maar weet niet waar wel...
    }
    else {theta1 = theta1;}*/
    //hieronder komen de doorrekeningen van de hoeken naar de coordinaten, check de mathematicafile voor de afleiding
    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);
    
        
        
        
        
    }//end van if xend !=xend
}
void demomode(){}       //Komt nog... 
                    //Alleen nodig in de 
                   //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);} // button2==0 moet veranderd naar emg3>= treshold
}*/ //suppressed omdat ik button 2 nodig heb...

// de beweging voor de xcoordinaat!
void inverse(){
    /*
    qn = qn-1 + (jacobian^-1)*dPref/dt *deltaT
    ofwel 
    thetai+1 = thetai +(jacobian)^-1*vector(deltaX, DeltaY)
    waar Pref = emg signaal
    */
    emg1 = false;                               
    if(theta10 == 0){             //als de hoek voor de beweging niet bekend is
        theta10 = PI*0.5;        //Definieer de hoek als half pi !!DIT KAN OOK ERGENS IN DEFAULT STAAN.
        theta1 = theta10;
    }
    if(button2 == 0){emg1 = true;
    
        if(emg1 == true){
            prefx = 1;                                                          //derivative of Preference = 1 (needs to be adjusted)
                theta1 = theta10 +/*inverse jacobian keer*/prefx*deltat;        //theta nieuw = oud +jacob*deltax
                theta10 = theta1;
        } 
    }//sluit van if emg1=true
    
    
    
}//sluit van void inverse

//dit is voor de ycoordinaat
void inverse2(){
    emg2 = false;                            //emg is het inputsignaal van de biceps
    if(theta40 == 0){
        theta40 = PI*0.5; 
        theta4 = theta40;
    }
    if(button1 == 0){ 
        emg2 = true;
        if(emg2 == true){
            prefy = 1;
                theta4 = theta40 + /*inverse jacobian keer*/prefy*deltat;        //theta nieuw = oud +jacob*deltax
                theta40 = theta4;
        }  //sluit van if emg2==true    
    }

}


int main()
{
    
    pc.baud(115200);
    //default = theta1 = theta4 = pi/2 
    emgcheck3.attach(forward, 0.1);
    emgcheck2.attach(inverse2, 0.1);
    emgcheck.attach(inverse, 0.1);
    ledr=1;
    pc.printf("%f", theta10);
    while(true){
        
        
           pc.printf("\n\r %f %f \n\r %f %f", theta4,theta1, xend, yend);
           wait(0.5);
           
    
    }
}