File content as of revision 3:611fd72c9d46:

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


#include "encoder.h"
#define K_P (0.1)
#define K_I (0.1)
#define K_D (0.0005 /TSAMP)
#define TSAMP 0.001
#define I_LIMIT 1.
#define THRESHOLD 0.02

#define K_P1 (0.004)
#define K_I1 (0.00001*TSAMP1)
#define K_D1 (0.0001/TSAMP1)
#define TSAMP1 0.01
#define I_LIMIT1 1.
// Alle constantes voor de filters definiëren
// Constantes voor de Low Pass filter
#define A1LP    0.018180963222803
#define A0LP     0.016544013176248
#define B1LP    -1.718913340044714
#define B0LP     0.753638316443765

// Constantes voor de High Pass Filter
#define A1HP    -1.999801878951505
#define A0HP   0.999801878951505
#define B1HP   -1.971717601075000
#define B0HP  0.972111984032897

// Constantes voor het Notch Filter
#define A0N 0.99436777112
#define A1N -1.89139989664
#define A2N 0.99436777112
#define B1N 1.89070035439
#define B2N -0.988036

// Sample Time
#define TSAMP 0.001
// Outputs van de motor
Encoder encoderA(PTD5,PTA13);
Encoder encoder1(PTD0,PTD2);

#include <iostream>

// Outputs naar de motor
PwmOut pwm(PTC8);
DigitalOut dir(PTC9);
PwmOut pwm1(PTA5);
DigitalOut dir1(PTA4);

//lampjes

DigitalOut rood(LED1);
DigitalOut blauw(LED3);
DigitalOut groen(LED2);
AnalogIn    emg1(PTB1);
AnalogIn    emg2(PTB2);
// Vaststellen van het datatype van de outputs naar de motor
float s_speed1;
float s_dir1;
float s_speed2;
float s_dir2;

// PC communicatie
MODSERIAL pc(USBTX,USBRX);

int set;

// Ticker voor de meetgegevens
Ticker timer;
volatile bool looptimerflag;

// Waardes voor de filters reserveren en als float vaststellen
float emg_value2, ylp2, yhp2, yn2,ysum1 = 0, yave1=0;
float emg_value1, ylp1, yhp1, yn1,ysum2 = 0, yave2=0 ;


//variabvelen voor positie motor1
float v1=0,out1 =0;
int pos1 =0,zero =0, a=0;
float out_i1 = 0;


// 0 of 1 waardes gedefinieerd uit het EMG, met 0 is te lage activiteit, 1 is hoog genoeg, voor de zekerheid toch als int.
uint8_t y1, y2;
int check = 0;

// integers reserveren voor het deel van het regelsysteem als we de boolean emg waardes hebben: hoek van het badje (0,1, of 2) en de gewenste snelheid (0,1, of 2)
int badjestand, badjes=1,badjedone=0,speeding,armstand=0,armspeed=0;
bool speeddone=0;

// Teller voor hoeveel metingen er zijn gedaan
uint16_t teller=0;

// Random meuk die ik uit batjespositie haal, nog ff opruimen
int32_t enc = 0,enca2 =0,enca1=0, encp=0, counts =0;
float speed = 0.1, out =0;
int pos =0;
float v=0;
float out_i = 0;
int y;

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


// EMG 1 uitlezen
float readEMG1()
{

    emg_value1=emg1.read();
    return emg_value1;
}

// EMG 2 uitlezen
float readEMG2()
{

    emg_value2=emg2.read();
    return emg_value2;
}

// Notch Filter voor EMG 1
float notchfilter1(float ylp1)
{
    static float yn1,x1=0,x2=0,y1=0,y2=0,x;
    x = ylp1;
    yn1 = A0N*x + A1N*x1+A2N*x2+B1N*y1+B2N*y2;
    x2 = x1;
    x1 = x;
    y2 = y1;
    y1 = yn1;
    return yn1;
}

// Notch Filter voor EMG 2 (idem aan die voor EMG 1, maar met eigen variabelen)
float notchfilter2(float ylp2)
{
    static float x1=0,x2=0,y1=0,y2=0,x;
    x = ylp2;
    yn2 = A0N*x + A1N*x1+A2N*x2+B1N*y1+B2N*y2;
    x2 = x1;
    x1 = x;
    y2 = y1;
    y1 = yn2;
    return yn2;
}

// High Pass Filter voor EMG 1
float hpfilter1(float yn1)
{
    static float x1=0,y1=0,x2=0, y2=0,x;
    x = yn1;
    yhp1 = x + A1HP*x1 + A0HP*x2 - B1HP*y1 - B0HP*y2;
    x2 = x1;
    x1 = x;
    y2 = y1;
    y1 = yhp1;
    return yhp1;
}

// High Pass Filter voor EMG 2 (idem aan die voor EMG 1, maar met eigen variabelen)
float hpfilter2(float yn2)
{
    static float x1=0,y1=0,x2=0, y2=0,x;
    x = yn2;
    yhp2 = x + A1HP*x1 + A0HP*x2 - B1HP*y1 - B0HP*y2;
    x2 = x1;
    x1 = x;
    y2 = y1;
    y1 = yhp2;
    return yhp2;
}

// Low Pass Filter voor EMG 1
float lpfilter1(float yhp1)
{
    static float x1=0,y1=0,x2=0, y2=0,x;
    x = yhp1;
    ylp1 = A1LP*x1-B1LP*y1+A0LP*x2-B0LP*y2;
    x2 = x1;
    x1 = x;
    y2 = y1;
    y1 = ylp1;
    return ylp1;
}

// Low Pass Filter voor EMG 2 (idem aan die voor EMG 1, maar met eigen variabelen)
float lpfilter2(float yhp2)
{
    static float x1=0,y1=0,x2=0, y2=0,x;
    x = yhp2;
    ylp2 = A1LP*x1-B1LP*y1+A0LP*x2-B0LP*y2;
    x2 = x1;
    x1 = x;
    y2 = y1;
    y1 = ylp2;
    return ylp2;
}

// Stukje nodig voor de ticker
void setlooptimerflag(void)
{
    looptimerflag = true;
}

// Combinatie van de drie bovengenoemde filters voor EMG 1, en de middeling van 500 dataputen (nieuwe data 2 keer per seconde)
float filter1(float emg_value1)
{
    static uint16_t n;
    yn1 = notchfilter1(emg_value1);
    yhp1 = hpfilter1(yn1);
    ylp1 = lpfilter1(yhp1);
    ylp1 = fabs(ylp1);
    ysum1 = ysum1+ylp1;
    n++;
    if(n==500) {
        yave1 = ysum1/500;
        ysum1 = 0;
        n = 0;
    }
    return yave1;
}

// Idem voor EMG 2
float filter2(float emg_value2)
{
    static uint16_t n;
    yn2 = notchfilter2(emg_value2);
    yhp2 = hpfilter2(yn2);
    ylp2 = lpfilter2(yhp2);
    ylp2 = fabs(ylp2);
    ysum2 = ysum2 + ylp2;
    n++;

    if(n==500) {
        yave2 = ysum2/500;
        ysum2 = 0;
        n = 0;
    }
    return yave2;
}

// Check of de waarde van het EMG boven de gedefinieerde threshold waarde uitkomt voor EMG 1
float threshold1 (float yave1)
{
    if(yave1>THRESHOLD) {
        y1 = 1;
    } else {
        y1 = 0;
    }
    return y1;
}

//Idem voor EMG 2
float threshold2 (float yave2)
{
    if(yave2>THRESHOLD) {
        y2 = 1;
    } else {
        y2 = 0;
    }
    return y2;
}

// Hoek van het badje vaststelen: begint met waarde 1, als EMG 1 alleen gebruikt wordt, gaat er 1 vanaf (maximaal 0), wordt EMG 2 alleen gebruikt, dan gaat er 1 bij (maximaal 2).
// Als beide tegelijkertijd 2 maal achter elkaar gebruikt worden, dan wordt de hoek vastgezet (kan niet meer verandert worden tot de reset).


// Stukje dat hetzelfde als bij de hoek van het badje regelt, maar dan voor de snelheid waarmee de arm moet bewegen. Heeft ook de waardes 0,1, of 2.
// Bij 2 maal achter elkaar EMG 1 en EMG 2 gebruikt, wordt de snelheid vastgezet.

void batposition(int y)
{
    switch(y) {
        case 0:
          
            dir = 1;
            pwm.write(0.4);
            wait(0.04);
            pwm.write(0);
            break;
        case 1:
            
            dir = 0;
            pwm.write(0.4);
            wait(0.04);
            pwm.write(0);
            break;
        case 2:
            
            pwm.write(0);
            break;

    }

}
float pid1(float setpoint, float measurement)
{
    float error;
    static float prev_error = 0;
    float           out_p = 0;
    float           out_d = 0;
    error  = setpoint-measurement;
    out_p  = error*K_P1;
    out_i1 += error*K_I1;
    out_d  = (error-prev_error)*K_D1;
    clamp(&out_i1,-I_LIMIT1,I_LIMIT1);
    prev_error = error;
    out1  = out_i1+out_p+out_d;
    return out1;
}

float getv1(float delta_t)
{
    int enca1,enca2,counts,enc;
    float v;
    int n =0 ;
    while(n<3) {
        wait(delta_t);
        enc = encoder1.getPosition();
        enca2 = enca1;
        enca1 = enc;
        n++;

    }

    counts = (enca1 - enca2)/delta_t;
    v = (counts)*((2*3.14159265359)/1550);
    return v;
}
float reset()
{
    v1 = 1;
    while(v1 !=0) {
        dir1 = 0;
        pwm1.write(0.1);
        v1 =getv1(0.1);
    }
    pwm1 = 0;
    dir1 =1;
    encoder1.setPosition(0);
   
    return pwm1;
}
uint8_t velocity (uint8_t y1, uint8_t y2)
{
    static int stand=0;
    if (y1>0 && y2>0 && check>0) {
        speeddone=1;
        check=0;
        rood = 0;
        groen = 1;
       cout<<"ga naar mode 1"<<endl;
    } else if (y1>0 && y2>0) {
        check=1;
    } else if (y1>0) {
        if (speed>0) {
            stand=stand+1;
            check=0;
            cout<<"stand "<<stand<<endl;
        }
    } else if (y2>0 ) {
        stand=stand-1;
        check=0;
        cout<<"stand "<<stand<<endl;
        }
     else{
         stand = 3;
         check =0;
         }
   
    return stand;
}

uint8_t badje (uint8_t y1, uint8_t y2)
{
    if (y1>0 && y2>0 && check>0) {
        badjedone=1;
        check=0;
        rood = 1;
        groen = 0;
        cout<<"ga naar mode 2"<<endl;
        
    } else if (y1>0 && y2>0) {
        check=1;
    } else if (y1>0) {
        badjes=0;
        check=0;
    } else if (y2>0 ) {
        badjes=1;
        check=0;
    }

    else {
        check=0;
        badjes = 2;
    }
    return badjes;
}

void gotopos(int pos1)
{
    int place;
    switch(pos1) {
        case 0:
           place = pos1;
            break;
        case 1:
            place = 100;
            break;

        case 2:
            place = 200;
            break;

        case 3:
            place = 250;
            break;
    }
    
    while((abs(place-encoder1.getPosition()) >6)|| (v1!= 0)) {

        while(!looptimerflag);
        looptimerflag = false;
        out1 = pid1(place,encoder1.getPosition());

        if(out1>0) {
            dir1 = 1;

        } else if(out1<0) {
            dir1 = 0;
        }
        pwm1 = fabs(out1);
        v1 = getv1(0.001);
    }
    pwm1 =0;

    cout<<"place done"<<endl;
}

// Main code
int main()
{
    reset();
    rood = 0;
    blauw = 1;
    groen = 1;
    // Startcondities
    pc.baud(115200);
    timer.attach(setlooptimerflag,TSAMP);
    // 20 secondes wachten, zodat er geen effecten van het opstarten van het bordje, alvast waardes voor het badje of de arm vastzetten.
    wait(2);
    cout<<"fase1"<<endl;
    while(1) {

        // Ticker
        while(!looptimerflag);

        looptimerflag = false;

        // EMG uitlezen
        emg_value1 =  readEMG1();
        emg_value2 =  readEMG2();

        // Filters gebruiken
        yave1 = filter1(emg_value1);
        yave2 = filter2(emg_value2);

        // Checken of de waardes boven de threshold uitkomen
        y1=threshold1(yave1);
        y2=threshold2(yave2);

        // Aantal metingen tellen
        teller++;

        // Om de 500 metingen controleren
        if (teller==500) {

            teller=0;
            // Als de hoek voor het badje nog niet klaar is, zet badje in de huidig aangegeven stand
            if (badjedone==0) {
                badjestand=badje(y1,y2);
                
                batposition(badjestand);
              
                // Als de hoek voor het badje wel klaar is:
            } else if (badjedone==1) {
                // Als de snelheid van de arm ook al klaar is, zorg ervoor dat de rest iet meer gebeurt
                if (speeddone==1) {
                    armspeed=armstand+1;
                    badjedone=0;
                    speeddone=0;
                    badjestand=1;
                    armstand=0;
                    // Anders, pas de hoek van de arm aan naar de stand die hoort bij de huidig geselecteerde snelheid
                } else {
                    armstand=velocity(y1,y2);
        
                    cout<<"armstand "<<armstand<<endl;
                    gotopos(armstand);
                }

            }
        }
        // Verzend data naar de scopes

        

    }

}