File content as of revision 0:ba5ff341b020:

#include "mbed.h"
#include "MODSERIAL.h"
#include "encoder.h"

// definieren constanten
#define PI 3.1415926
//plant
#define ARM1 0.36
#define ARM2 0.18
//PD
#define CP 0.0002
#define CD 0.002
#define CLP1 0.9975
#define CLP2 0.001
//Snelheid
#define SNELHEID 0.05
//LOOPTIME
#define LOOPTIME 0.01
//Filtering EMG
#define HP1 0.8187
#define HP2 20.0
#define HP3 20.0
#define LP1 0.9512
#define LP2 0.04877

void keep_in_range(float * in, float min, float max);

volatile bool looptimerflag;

void setlooptimerflag(void)
{
    looptimerflag = true;
}

int main() {
    AnalogIn EMG0(PTB0);
    AnalogIn EMG1(PTB1);
    AnalogIn EMG2(PTB2);
    AnalogIn EMG3(PTB3);
    AnalogIn potmeter(PTC2);
    DigitalIn ButtonUP(PTE20); //Knopjes voor kalibratie
    DigitalIn ButtonDOWN(PTE21);
    DigitalIn ButtonSELECT(PTE22);
    DigitalIn ButtonSTOP(PTE23);
    DigitalOut Solenoid(PTD4); //Solenoid
    Encoder motor1(PTD0,PTD2);//Moet aangesloten zijn op twee pinnen, waarvan een met interrupt mogelijkheid
    Encoder motor2(;
    PwmOut pwm_motor1(PTA12); 
    DigitalOut motordir1(PTD3);
    PwmOut pwm_motor2(PTA5);
    DigitalOut motordir2(PTD1);
    
    //Variabelen verwerking EMG
    float emg_value1, emg_value2, emg_value3, emg_value4;
    float emg_value1min1=0.0, emg_value2min1=0.0, emg_value3min1=0.0, emg_value4min1=0.0;
    float EMGhp1, EMGhp2, EMGhp3, EMGhp4, EMGlp1, EMGlp2, EMGlp3, EMGlp4;
    float EMGhp1min1=0.0, EMGhp2min1=0.0, EMGhp3min1=0.0, EMGhp4min1=0.0, EMGlp1min1=0.0, EMGlp2min1=0.0, EMGlp3min1=0.0, EMGlp4min1=0.0;
    float EMGmax1, EMGmin1, EMGmax2, EMGmin2, EMGmax3, EMGmin3, EMGmax4, EMGmin4;

    //Variabelen bepaling input systeem
    float input;
    float w1, w2, wM2, phi1, phi2, theta;
    float a, b, c, d, ai, bi, ci, di;
    float v1, v2, v3, v4, vx, vy;
    
    //Variabelen motoraansturing
    float setpointM1, setpointM2;
    float setpointmin1M1=0.0, setpointmin1M2=0.0;
    float pwm_to_motor1, pwm_to_motor2;
    float M1position, M2position;
    float foutM1, foutM2;
    float foutmin1M1=0.0, foutmin1M2=0.0;
    float foutverschilM1, foutverschilM2;
    float foutverschilmin1M1=0.0, foutverschilmin1M2=0.0;
    float CDloop=CD/LOOPTIME;

    //Kalibratie
    //1. Bepalen EMGmax1 EMGmin1
    //2. Bepalen EMGmax2 EMGmin2
    //3. Bepalen EMGmax3 EMGmin3
    //4. Bepalen EMGmax4 EMGmin4
    EMGmax1=+(potmeter.read()-0.5)*;
    EMGmin1=+(potmeter.read()-0.5)*;
    EMGmax2=+(potmeter.read()-0.5)*;
    EMGmin2=+(potmeter.read()-0.5)*;
    EMGmax3=+(potmeter.read()-0.5)*;
    EMGmin3=+(potmeter.read()-0.5)*;
    EMGmax4=+(potmeter.read()-0.5)*;
    EMGmin4=+(potmeter.read()-0.5)*;
    
    //Aansturing
    Ticker looptimer;
    looptimer.attach(setlooptimerflag,LOOPTIME);  
    while(1) {
        while(looptimerflag != true);
        looptimerflag = false;
        
        //uitlezen
        emg_value1 = EMG0.read();
        emg_value2 = EMG1.read();
        emg_value3 = EMG2.read();
        emg_value4 = EMG3.read();
        
        //filtering
        EMGhp1=HP1*EMGhp1min1+HP2*emg_value1-HP3*emg_value1min1;
        EMGhp2=HP1*EMGhp2min1+HP2*emg_value2-HP3*emg_value2min1;
        EMGhp3=HP1*EMGhp3min1+HP2*emg_value3-HP3*emg_value3min1;
        EMGhp4=HP1*EMGhp4min1+HP2*emg_value4-HP3*emg_value4min1;
        EMGhp1=abs(EMGhp1);
        EMGhp2=abs(EMGhp2);
        EMGhp3=abs(EMGhp3);
        EMGhp4=abs(EMGhp4);
        EMGlp1=LP1*EMGlp1min1+LP2*EMGhp1min1;
        EMGlp2=LP1*EMGlp2min1+LP2*EMGhp2min1;
        EMGlp3=LP1*EMGlp3min1+LP2*EMGhp3min1;
        EMGlp4=LP1*EMGlp4min1+LP2*EMGhp4min1;
        
        //berekenen setpoint
        //hoekinput
        v1=(EMGlp1-EMGmin1)/(EMGmax1-EMGmin1);
        v2=(EMGlp2-EMGmin2)/(EMGmax2-EMGmin2);
        v3=(EMGlp3-EMGmin3)/(EMGmax3-EMGmin3);
        v4=(EMGlp4-EMGmin4)/(EMGmax4-EMGmin4);
        input=tan((v1-v2)/(v3-v4));
        
        //snelheidsvector
        vx=cos(input)*SNELHEID;
        vy=sin(input)*SNELHEID;
        
        //input positie
        phi1=motor1.getPosition();// Integraal van ideale positie maken!!!!!!!!!!
        theta=motor2.getPosition();
        phi2=theta+phi1-PI;
        
        //Jacobiaan
        // [a  b]
        // [c  d]
        a=cos(phi1)*ARM1;
        b=sin(phi1)*ARM1;
        c=cos(phi2)*ARM2;
        d=sin(phi2)*ARM2;
        
        //inverse
        // [ai  bi]
        // [ci  di]
        ai=d/(a*d-b*c);
        bi=-b/(a*d-b*c);
        ci=-c/(a*d-b*c);
        di=a/(a*d-b*c);
        
        //vermenigvuldiging
        // [ai  bi]   [vx]   [w1]
        // [ci  di] * [vy] = [w2]
        w1=ai*vx+bi*vy; //=wM1 hoeksnelheid van motor 1
        w2=ci*vx+di*vy;
        wM2=w2-w1;//hoeksnelheid motor 2
    
        //Motoraansturing
        setpointM1 = (w1/(2.0*PI))*3200.0*LOOPTIME+setpointmin1M1;
        setpointM2 = (wM2/(2.0*PI))*3200.0*LOOPTIME+setpointmin1M2;
        M1position = motor1.getPosition();
        M2position = motor2.getPosition();
        foutM1 = setpointM1-M1position;
        foutM2 = setpointM2-M2position;
        foutverschilM1 = foutM1-foutmin1M1;
        foutverschilM2 = foutM2-foutmin1M2;
        foutverschilM1 = CLP1*foutverschilmin1M1+CLP2*foutverschilM1;
        foutverschilM2 = CLP1*foutverschilmin1M2+CLP2*foutverschilM2;
        pwm_to_motor1 = foutM1*CP+foutverschilM1*CDloop;
        pwm_to_motor2 = foutM2*CP+foutverschilM2*CDloop;
        keep_in_range(&pwm_to_motor1, -1,1);
        keep_in_range(&pwm_to_motor2, -1,1);
        keep_in_range(&setpointM1, -800,800);//Juiste hoeken invoeren
        keep_in_range(&setpointM2, 0,1500);
        if(pwm_to_motor1 > 0)
            motordir1 = 0;
        else
            motordir1 = 1;
        if(pwm_to_motor2 > 0)
            motordir2 = 0;
        else
            motordir2 = 1;
        
        //WRITE VALUE TO MOTOR  
        pwm_motor1.write(abs(pwm_to_motor1));
        pwm_motor2.write(abs(pwm_to_motor2));
        
        //Definieren waarden in de verleden tijd
        foutmin1M1=foutM1;
        foutmin1M2=foutM2;
        foutverschilmin1M1=foutverschilM1;
        foutverschilmin1M2=foutverschilM2;
        setpointmin1M1=setpointM1;     
        setpointmin1M2=setpointM2;        
    }
}       

void keep_in_range(float * in, float min, float max)
{
    *in > min ? *in < max? : *in = max: *in = min;
}