File content as of revision 2:4ae32e8863d5:

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

/** keep_in_range -> float in, and keep_in_range if less than min, or larger than max **/
void keep_in_range(float * in, float min, float max);

/** variable to show when a new loop can be started*/
/** volatile means that it can be changed in an    */
/** interrupt routine, and that that change is vis-*/
/** ible in the main loop. */

volatile bool looptimerflag;

/** function called by Ticker "looptimer"     */
/** variable 'looptimerflag' is set to 'true' */
/** each time the looptimer expires.          */
void setlooptimerflag(void)
{
    looptimerflag = true;
}


int main()
{

    //LOCAL VARIABLES
    /*Potmeter input*/
    //AnalogIn potmeter(PTC2);
    //AnalogIn potmeter2(PTB0);

    //EMG input
    AnalogIn emg0(PTB0);
    AnalogIn emg1(PTB1);
    AnalogIn emg2(PTB2);
    AnalogIn emg3(PTB3);
    /* Encoder, using my encoder library */
    /* First pin should be PTDx or PTAx  */
    /* because those pins can be used as */
    /* InterruptIn                       */
    Encoder motor1(PTD0,PTC9);
    Encoder motor2(PTD2,PTC8);
    /* MODSERIAL to get non-blocking Serial*/
    MODSERIAL pc(USBTX,USBRX);
    /* PWM control to motor */
    PwmOut pwm_motor(PTA12);
    PwmOut pwm_motor2(PTA5);
    /* Direction pin */
    DigitalOut motordir(PTD3);
    DigitalOut motordir2(PTD1);
    

    /*floats van de filters van het emg*/
    pc.baud(115200);   // Defining the Baud rate for comunication with PC
    /*EMG Variables 1*/
    float t;
        t = 0;
        
    float hoek1, hoek2;
    hoek1 = 0;
    hoek2 = 0;
    /*Floats voor Maximal value*/
    
    float h,h1,hh,hh1,b,bb;
    h=0;
    h1=0;
    hh=0;
    hh1=0;
    b=0;
    bb=0;
    
    
    float x,y,z,x1,x2,y1,z1,y2,z2,f,f1,f2,s,s1 ;      //Defining variables for filter
    x1=0;              // setting the variables to zero so they don't
    x2=0;              // have any value jet and are used to store older values later
    y1=0;
    y2=0;
    z1=0;
    z2=0;
    f1=0;
    f2=0;
    s=0;
    s1=0;
    const float Ts = 0.01;      //Defining time steps
    
    /*EMG Variables 2*/
    float xx,yy,zz,xx1,xx2,yy1,zz1,yy2,zz2,ff,ff1,ff2 ;      //Defining variables for filter
    xx1=0;              // setting the variables to zero so they don't
    xx2=0;              // have any value jet and are used to store older values later
    yy1=0;
    yy2=0;
    zz1=0;
    zz2=0;
    ff1=0;
    ff2=0;

    /*EMG Variables 3*/
    float xxx,yyy,zzz,xxx1,xxx2,yyy1,zzz1,yyy2,zzz2,fff,fff1,fff2 ;      //Defining variables for filter
    xxx1=0;              // setting the variables to zero so they don't
    xxx2=0;              // have any value jet and are used to store older values later
    yyy1=0;
    yyy2=0;
    zzz1=0;
    zzz2=0;
    fff1=0;
    fff2=0;

    /*EMG Variables 4*/
float xxxx,yyyy,zzzz,xxxx1,xxxx2,yyyy1,zzzz1,yyyy2,zzzz2,ffff,ffff1,ffff2 ;      //Defining variables for filter
    xxxx1=0;              // setting the variables to zero so they don't
    xxxx2=0;              // have any value jet and are used to store older values later
    yyyy1=0;
    yyyy2=0;
    zzzz1=0;
    zzzz2=0;
    ffff1=0;
    ffff2=0;
    float F,FF,FFF,FFFF;
    /*Variables to store direction in dependent on the EMG*/
    float emg_dir;
    float emg_dir2;
    float emgPos1, emgPos2, emgPos10, emgPos20;
    emgPos10 = 0;
    emgPos20 = 0;
    
    /* variable to store setpoint in */
    float setpoint;
    float setpoint2;
    
    /* variable to store pwm value in*/
    float pwm_to_motor;
    float pwm_to_motor2;
       
    
    /* Alle waardes voor de regelaar benoemen. Moet nog afgesteld worden*/
    float u, u1, e, e1, ui, ei, up, ei1, ed, ud;
    float u2, u12, e2, e12, ui2, ei2, up2, ei12, ed2, ud2;
    const float kp = 0.001;//0.02438;     //test value 0.001
    const float ki = 0.00001;//0.11661;   //test value 0.00001
    const float kd = 0.00001;//0.00071;   //test value 0.00001
    //const float Ts = 0.01;
    const float kp2 = 0.001;//0.02274;    //test value 0.001
    const float ki2 = 0.00001;//0.10879;  //test value 0.00001
    const float kd2 = 0.00001;//0.00065;  //test value 0.00001
    //const float Ts2 = 0.1;
    e1 = 0;
    u1 = 0;
    ei1 = 0;
    e12 = 0;
    u12 = 0;
    ei12 = 0;
    //START OF CODE

    /*Set the baudrate (use this number in RealTerm too! */
    //pc.baud(115200);

    /*Create a ticker, and let it call the     */
    /*function 'setlooptimerflag' every 0.01s  */
    Ticker looptimer;
    looptimer.attach(setlooptimerflag,Ts);
    

    //INFINITE LOOP
    while(1) {
        /* Wait until looptimer flag is true. */
        /* '!=' means not-is-equal            */
        while(looptimerflag != true);
        /* Clear the looptimerflag, otherwise */
        /* the program would simply continue  */
        /* without waitingin the next iteration*/
        looptimerflag = false;
        
            /*while loop voor de emg*/
    
        /* EMG Filter 1*/
        x = emg0.read();                    //Reading EMG value
        y = 0.6389*x+1.2779*x1+0.6389*x2-y1*1.143-y2*0.4128;    //Formula for highpass filter at 20Hz as given in slides
        z = y*0.3913+y1*-0.7827+y2*0.3913-z1*-0.3695-z2*0.1958; // Formula for low pass filter at 40Hz instead of a Notch filter
        z = abs(z); //Rectify the signal
        f = z*0.0036+z1*0.0072+z2*0.0036-f1*-1.8227-f2*0.8372;   // low pass filter at 5Hz
        F = f;
        
        
        z1 = z;                 // Store older values of variables
        z2 = z1;
        y2 = y1;
        y1 = y;
        x1 = x;
        x2 = x1;
        f1 = f;
        f2 = f1;
        
        if (f<0.2)
        f=0;
        else 
        f=f;
        
         //pc.printf("%f \n \r",(s1*f));
        
        /* EMG Filter 2*/
        xx = emg1.read();                    //Reading EMG value
        yy = 0.6389*xx+1.2779*xx1+0.6389*xx2-yy1*1.143-yy2*0.4128;    //Formula for highpass filter at 20Hz as given in slides
        zz = yy*0.3913+yy1*-0.7827+yy2*0.3913-zz1*-0.3695-zz2*0.1958; // Formula for low pass filter at 40Hz instead of a Notch filter
        zz = abs(zz); //Rectify the signal
        ff = zz*0.0036+zz1*0.0072+zz2*0.0036-ff1*-1.8227-ff2*0.8372;   // low pass filter at 5Hz
        FF = ff;

        zz1 = zz;                 // Store older values of variables
        zz2 = zz1;
        yy2 = yy1;
        yy1 = yy;
        xx1 = xx;
        xx2 = xx1;
        ff1 = ff;
        ff2 = ff1;
        if (ff<0.2)
        ff=0;
        else 
        ff=ff;
        /* EMG Filter 3*/
        xxx = emg2.read();                    //Reading EMG value
        yyy = 0.6389*xxx+1.2779*xxx1+0.6389*xxx2-yyy1*1.143-yyy2*0.4128;    //Formula for highpass filter at 20Hz as given in slides
        zzz = yyy*0.3913+yyy1*-0.7827+yyy2*0.3913-zzz1*-0.3695-zzz2*0.1958; // Formula for low pass filter at 40Hz instead of a Notch filter
        zzz = abs(zzz); //Rectify the signal
        fff = zzz*0.0036+zzz1*0.0072+zzz2*0.0036-fff1*-1.8227-fff2*0.8372;   // low pass filter at 5Hz
        FFF = fff;

        zzz1 = zzz;                 // Store older values of variables
        zzz2 = zzz1;
        yyy2 = yyy1;
        yyy1 = yyy;
        xxx1 = xxx;
        xxx2 = xxx1;
        fff1 = fff;
        fff2 = fff1;
        if (fff<0.1)
        fff=0;
        else 
        fff=fff;
        /* EMG Filter 4*/
        xxxx = emg3.read();                    //Reading EMG value
        yyyy = 0.6389*xxxx+1.2779*xxxx1+0.6389*xxxx2-yyyy1*1.143-yyyy2*0.4128;    //Formula for highpass filter at 20Hz as given in slides
        zzzz = yyyy*0.3913+yyyy1*-0.7827+yyyy2*0.3913-zzzz1*-0.3695-zzzz2*0.1958; // Formula for low pass filter at 40Hz instead of a Notch filter
        zzzz = abs(zzzz); //Rectify the signal
        ffff = zzzz*0.0036+zzzz1*0.0072+zzzz2*0.0036-ffff1*-1.8227-ffff2*0.8372;   // low pass filter at 5Hz
        FFFF = ffff;

        zzzz1 = zzzz;                 // Store older values of variables
        zzzz2 = zzzz1;
        yyyy2 = yyyy1;
        yyyy1 = yyyy;
        xxxx1 = xxxx;
        xxxx2 = xxxx1;
        ffff1 = ffff;
        ffff2 = ffff1;
        if (ffff<0.1)
        ffff=0;
        
        
        else 
        ffff=ffff;
        //Printing the Filtered singnal
        //pc.printf("%f \n \r",f);

        wait(Ts);
        /*Defining the direction of the Motor*/
        emg_dir=f;/*-ff; snelheid in de x richting. Of draaiing motor 1*/
        emg_dir2=fff;/*-ffff; /*snelheid in de y richting. Of draaiing motor2*/
        
        //omzetten naar hoeksnelheden
        //hoekV1 = emg_dir*29.5*cos(hoek2)-emg_dir2*29,5*sin(hoek2); //nu heb ik beide hoeksnelheden
        //hoekV2 = -emg_dir*21,5*cos(hoek1)+emg_dir2*21,5*sin(hoek1);
        
         
        /*emgPosx1 = emg_dir2*Ts + emgPos10;
        emgPos2 = emg_dir2*Ts + emgPos20;
        emgPos10 = emgPos1;
        emgPos20 = emgPos2;*/
        
        /* transformatie van xy naar hoek1 en hoek2*/
        
        //t+=Ts;
        /*Defining which way to turn*/
        
        
        if (f>0.2){         //Defining a threshhold abvoe which theres unlikely to measure any activity of other muscles
        if (f>h);           //Looking for the biggest value of f
        h=(f);                //Storing it in h 
        if (h1<h)           //if h is a value below the set maximum
        h1=h1+0.001;        //the variable of the setpoint will try to narrow it in steps of 0.005
        }
        
        if (ff>0.2){
        if (ff>b)
        b=(1-ff);
        if (h1>b)
        h1=h1-0.001;
        }
        keep_in_range(&h1,0,1);
        
        if (fff>0.2){
        if (fff>hh);
        hh=fff;
        if (hh1<hh)
        hh1=hh1+0.005;
        }
        
        if (ffff>0.2){
        if (ffff>bb)
        bb=(1-ffff);
        if (hh1>bb)
        hh1=hh1-0.005;
        }
        keep_in_range(&hh1,0,1);
        //emgPosx = 0.5*sin(t);
        //emgPosy = 0.5*cos(t);
        
        //emgPos1 = 
        //emgPos2 = 
        
        
        //pc.printf("%f \n \r",emgPos1);
        /* Read EMDG values, apply some math */
        /* to get useful setpoint value         */
        /*setpoint = ((ff)-0.2255)*1226.55;*/ /* SHOUDER kan van -23 tot 79 graden draaien*/
        pc.printf("%f,%f \n \r",f,ff);
        setpoint = (h1*1226.55); //emgPos moet wel tussen 0 en 1 zitte?
        setpoint2 = (hh1*1226.55);
        /*setpoint2 = (potmeter2.read())*1226,55; ELLEBOOG kan van 0 tot 102 graden draaien*/
       
        // Print setpoint and current position to serial terminal
        //pc.printf("%f,%f,%f,%d \n \r",f,ff,emg_dir,motordir.read());
        /*pc.printf("%f, %f \r\n", motor2.getSpeed(),f);*/
        /*pc.printf(" %f, %d, %f, %d \n\r", setpoint, motor1.getPosition(), setpoint2, motor2.getPosition());*/
        /*pc.printf("s: %f, %d \n\r", setpoint2, motor2.getPosition());*/

        /*This is a P-action! calculate error, multiply with gain, and store in pwm_to_motor*/

        /* De PID berekeningen*/
        e = setpoint - motor1.getPosition();
        up = kp*e;
        ei = e*Ts + ei1;
        ui = ki*ei;
        keep_in_range(&ui, -0.5*setpoint,0.5*setpoint);
        ed = (e-e1)/Ts;
        ud = ed*kd;
        u = up + ui + ud;
        /*pwm_to_motor = 0;*/
        pwm_to_motor = u;

        u1= u;
        e1= e;
        ei1 = ei;

        /* Het is nu een PID actie geworden voor motor 2*/
        e2 = setpoint2 - motor2.getPosition();
        up2 = kp2*e2;
        ei2 = e2*Ts + ei12;
        ui2 = ki2*ei2;
        keep_in_range(&ui, -0.5*setpoint2,0.5*setpoint2);
        ed2 = (e2-e12)/Ts;
        ud2 = ed2*kd2;
        u2 = up2 + ui2 + ud2;
        /*u = (kp+ki*Ts)*e-kp*e1+u1;*/
        pwm_to_motor2 = u2;
        /*pwm_to_motor2 = 0;*/

        u12= u2;
        e12= e2;
        ei12 = ei2;

        //Make sure the PWM stays in range
        keep_in_range(&pwm_to_motor, -1,1);
        keep_in_range(&pwm_to_motor2, -1,1);
        
        /* Control the motor direction pin. based on   */
        /* the sign of your pwm value.      If your    */
        /* motor keeps spinning when running this code */
        /* you probably need to swap the motor wires,  */
        /* or swap the 'write(1)' and 'write(0)' below */
        if(pwm_to_motor > 0){ //if (pwm_to_motor > 0) emg_dir  > 0{
            motordir.write(1);
            }
        else{
            motordir.write(0);
            }
       
        //WRITE VALUE TO MOTOR
        /* Take the absolute value of the PWM to send */
        /* to the motor. */
        pwm_motor.write(abs(pwm_to_motor));//pwm_motor.write(0);
                
        if(pwm_to_motor2 > 0){ //if (pwm_to_motor2 > 0)emg_dir2 > 0{  
            motordir2.write(1);
            }
        else{
            motordir2.write(0);
            }    
        //pwm_motor2.write(abs(pwm_to_motor2));
        pwm_motor2.write(abs(pwm_to_motor2));
        //pc.printf("f:%f, emg_dir:%f, setpoint:%f, u:%f, pwm_to_motor:%d, motordir:%d \n \r",f,emg_dir,setpoint,u,pwm_to_motor,motordir.read());
         //pc.printf("f:%f, ff:%f, h1:%f, h:%f, b:%f, Pos:%f \n \r",f,ff,h1,h,b,motor2.getPosition());
    }
}


//coerces value 'in' to min or max when exceeding those values
//if you'd like to understand the statement below take a google for
//'ternary operators'.
void keep_in_range(float * in, float min, float max)
{
*in > min ? *in < max? : *in = max: *in = min;
}