File content as of revision 10:e628a87d18aa:

//ROBOTCONTROL BMT M9 - GROEP 2
//Alex Overbeek
//Tom Baumeister
//Bas van Buuren
//Bas Mattern
//Thijs ruikes

/*Het script bestaat uit drie delen: INCLUDE AND DEFINE, FUNCTIONS, MAINSCRIPT. Per gedeelte wordt uitgelegd wat gedaan wordt.
We laten de beweging in fases lopen. Fase 1 werkt in dit script. Voor fase 2 kan de positie al worden ingesteld, de slagbeweging
moet nog gemaakt worden. Dit gaan we doen met een PID regelaar die naar een snelheid toe regelt.
*/

//INCLUDE AND DEFINE ALL
#include "mbed.h"
#include "MODSERIAL.h"
#include <iostream>
#include "encoder.h"
#include "HIDScope.h"

#define THRESHOLD 0.04
#define NOSAMPL 500

#define TSAMP1 0.01
#define K_P1 (0.004)
#define K_I1 (0.00001*TSAMP1)
#define K_D1 (0.0001/TSAMP1)
#define I_LIMIT1 1.

//KPID voor slagfunctie
#define KSLA_P (0.05)
#define KSLA_I (0.000001 *TSAMP1)
#define KSLA_D (0.0005 /TSAMP1)

#define MAXPOS 700

// 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);

// 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);

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

// 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,out_i1 = 0; //out_i1 globaal gedef om reset

// 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;

//Definieer alle functies
void clamp(float * in, float min, float max);
void clampint(int * in, int min, int max);
void setlooptimerflag(void);
float readEMG1();
float readEMG2();
float notchfilter1(float ylp1);
float notchfilter2(float ylp2);
float hpfilter1(float yn1);
float hpfilter2(float yn2);
float lpfilter1(float yhp1);
float lpfilter2(float yhp2);
float filter1(float emg_value1);
float filter2(float emg_value2);
float threshold1 (float yave1);
float threshold2 (float yave2);
float getv(float delta_t);
float resetarm();
uint8_t badje (uint8_t y1, uint8_t y2);
void batposition(int y);
void sla(int k);
float pidposition(float setpoint, float measurement);
float pidarm(float rev_value, float mea_value);
uint8_t armposition (uint8_t y1, uint8_t y2);

/*FUNCTIES
In deze sectie worden de functie geprogrammeerd, hieronder een uitleg per functie. De functies zijn verdeeld in drie groepen. In het script
staat achter enkele functies een 1 of een 2. Er worden 2 signalen bewerkt, dus sommige functies staan dubbel in het script. Signaal 1 wordt
door de functies met 1, en signaal 2 wordt door de functie met 2, bewerkt.

GLOBAL FUNCTIONS
Clamp - "clampt" een waarde binnen grenzen, als de geclampte variabele over de grens heen gaat krijgt de variabele de grenswaarde.
setLooptimerflag - Hiermee wordt de ticker aangeroepen.

EMG FUNCTIONS
readEMG - Leest het voltage af van de geselecteerde pin, in ons geval een EMG waarde.
(notch/lp/hp)filter -Er worden drie filters gebruikt: lowpass, highpass en notchfilter. De funtie krijgt een input en geeft dan een gefilterde output.
filter - Voert de notch,lp,hp filters achter elkaar uit op een input en geeft het gemiddelde van 500 meetpunten als output.
threshold - Vergelijkt de input met een treshold, als de waarde boven de treshold komt (zodra een spier wordt aangespannen) wordt de output 1, anders 0.

MOTORCONTROLFUNCTIONS
resetarm - Brengt arm terug naar de nul positie, zet de encoder op 0.
getv(delta_t) - Geeft de huidige draaisnelheid van motor1, delta_t is het tijdsinterval waarover gemeten wordt
badje - Vergelijkt met dubbele input, y1 en y2. Er zijn vier situaties, beide = 0, y1=1, y2=1, beide =1.
badposition - Leest de output van badje, laat het badje links, recht, niet draaien.
pid... - Pid regelaar, de k waardes wordenin de define sectie gedefineerd.
armposition - Op basis van de emgsignalen wordt hier een positie ingesteld.
armtopos - Regelt de arm naar de opgegeven positie toe.
*/
//HIDScope scope(5);
//GLOBAL FUNCTIOMS
/*void viewer(){
    scope.set(0,yave1);
    scope.set(1,y1);
    scope.set(2,yave2);
    scope.set(3,y2);
  scope.send();
    }*/

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

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

void setlooptimerflag(void)
{
    looptimerflag = true;
}

//EMG FUNCTIONS
float readEMG1()
{
    emg_value1=emg1.read();
    return emg_value1;
}

float readEMG2()
{
    emg_value2=emg2.read();
    return emg_value2;
}

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;
}

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;
}

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;
}

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;
}

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;
}

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;
}

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;
}

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==NOSAMPL) {
        yave2 = ysum2/NOSAMPL;
        ysum2 = 0;
        n = 0;
    }
    return yave2;
}

float threshold1 (float yave1)
{
    if(yave1>THRESHOLD) {
        y1 = 1;
    } else {
        y1 = 0;
    }
    return y1;
}

float threshold2 (float yave2)
{
    if(yave2>THRESHOLD) {
        y2 = 1;
    } else {
        y2 = 0;
    }
    return y2;
}

//MOTORCONTROL FUNCTIONS
float getv(float delta_t)
{
    int enca1=0,enca2=0,counts=0;
    float v;
    int n =0 ;
    while(n<3) {
        wait(delta_t);
        enca2 = enca1;
        enca1 = encoder1.getPosition();
        n++;
    }
    counts = (enca1 - enca2)/delta_t;
    v = (counts)*((2*3.14159265359)/1550)*0.5;
    return v;
}

float resetarm()
{
    v1 = 1;
    while(v1 !=0) {
        dir1 = 0;
        pwm1.write(0.1);
        v1 =getv(0.1);
    }
    pwm1 = 0;
    dir1 =1;
    encoder1.setPosition(0);
    return pwm1;
}
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;
        wait(0.2);
    } 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 batposition(int y)
{
    switch(y) {
        case 0:

            dir = 1;
            pwm.write(0.4);
            wait(0.04);
            pwm.write(0);
            cout<<"links"<<endl;
            break;
        case 1:
            dir = 0;
            pwm.write(0.4);
            wait(0.04);
            pwm.write(0);
            cout<<"rechts"<<endl;
            break;
        case 2:
            pwm.write(0);
            break;

    }

}

void sla(int k)
{   
    float maxpwm;
    float new_pwm;
    switch(k)
    {
        case 1:
            maxpwm=0.2; //DE MAX PWM'en AANPASSEN VOOR DE ANDERE CASES!!!!
            break;
        case 2:
            maxpwm=0.2;
            break;
        case 3:
            maxpwm=0.2;
            break;
        case 4:
            maxpwm=0.2;
            break;
        default:
            maxpwm=0;
            break;
    }
    while((encoder1.getPosition()<MAXPOS)||getv(0.01)>0.1)
    {
        new_pwm=pidarm(MAXPOS,encoder1.getPosition());
        clamp(&new_pwm,-maxpwm,maxpwm);
        if (new_pwm>0)
        {
            dir1=1;
        }
        else if (new_pwm<0)
        {
            dir1=0;
        }
        pwm1.write(fabs(new_pwm));
        cout<<"pwm1: "<<new_pwm<<endl;
        cout<<"pos: "<<encoder1.getPosition()<<endl;
    }
      
 pwm1.write(0);
 wait(2);
 //cout<< encoder1.getPosition()<<endl;
}

float pidposition(float setpoint, float measurement)
{
    float error, out, out_p=0,out_d=0;
    static float prev_error = 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;
    out  = out_i1+out_p+out_d;
    cout<<"out: "<<out<<endl;
    return out;
}

float pidarm(float rev_value, float mea_value)
{
    float error;
    static float prev_error = 0;
    float p_out = 0;
    static float i_out = 0;
    float d_out = 0;
    error = rev_value - mea_value;
    p_out = error * KSLA_P;
    i_out += error * KSLA_I;
    d_out = (error - prev_error) * KSLA_D;
    clamp(&i_out,-I_LIMIT1,I_LIMIT1);
    prev_error=error;
    return p_out + i_out + d_out;
}


uint8_t armposition (uint8_t y1, uint8_t y2)
{
    static int stand=0;
    if (y1>0 && y2>0 && check>0) {
        badjedone=2;
        check=0;
        /*rood = 0;
        groen = 1;*/
        cout<<"ga naar mode 1"<<endl;
    } else if (y1>0 && y2>0) {
        check=1;
        cout<<"zo naar mode 1"<<endl;
    } else if (y1>0) {
        stand=stand+1;
        check=0;
        cout<<"stand "<<stand<<endl;
    } else if (y2>0 ) {
        stand=stand-1;
        check=0;
        cout<<"stand "<<stand<<endl;
    } else {
        
        check =0;
    }
    clampint(&stand,1,3);
    return stand;
}

float gotopos(int pos)
{
    float out1;
    
    switch(pos) {
        case 1:
            pos = 100;
            break;

        case 2:
            pos = 200;
            break;

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

        while(!looptimerflag);
        looptimerflag = false;
        out1 = pidposition(pos,encoder1.getPosition());

        if(out1>0) {
            dir1 = 1;

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

    return pwm1;
}

// MAIN SCRIPT
int main()
{
    /*rood = 0;
    blauw = 1;
    groen = 1;*/
    resetarm();
    pc.baud(115200);
    timer.attach(setlooptimerflag,TSAMP);
    wait(2);
    cout<<"Begin programma"<<endl;
    while(1) {
        //Per TSAMP word EMG uitgelzen, gefilterd en gemiddeld
        while(!looptimerflag);
        looptimerflag = false;
        emg_value1 =  readEMG1();
        emg_value2 =  readEMG2();
        yave1 = filter1(emg_value1);
        yave2 = filter2(emg_value2);
        y1=threshold1(yave1);
        y2=threshold2(yave2);
        teller++;

        /*Dit gedeelte voert de bewegingen uit. In de eerste fase kan met de linker spier het batje naar links gedraaid worden,
        en met de rechter spier het batje naar recht. Als het badje in de juiste positie staat kunnen beide spieren tegelijk
        aangespannen worden om naar de volgende fase te gaan. In fase 2 wordt de armpositie ingesteld. Hoe groter de afstand van de
        arm met de bal, hoe harder de arm gaat slaan. Zodra de arm in de goede positie staat kan door beide spieren aan te spannen worden
        doorgegaan naar fase 3. In fase 3 slaat de arm de bal. De snelheid die ingesteld wordt waarmee de arm gaat slaan wordt bepaald aan de
        hand van de positie waarin de arm in fase 2 is gezet. Zodra de arm de bal geslagen heeft moet de arm gereset worden en terug gaan naar
        fase 1.*/

        if (teller==NOSAMPL) {
            teller=0;
            switch(badjedone) {
                case 0:
                    rood=0;
                    groen=1;
                    blauw=1;
                    cout<<"fase1"<<endl;
                    badjestand=badje(y1,y2);
                    batposition(badjestand);
                    break;

                case 1:
                    rood=1;
                    groen=0;
                    blauw=1;
                    cout<<"fase2"<<endl;
                    armstand=armposition(y1,y2);
                    gotopos(armstand);
                    cout<<"armstand "<<armstand<<endl;
                    
                    //armtopos(armstand);
                    //cout<<"badjedont: "<<badjedone<<endl;
                    break;

                case 2:
                    rood=1;
                    groen=1;
                    blauw=0;
                    resetarm();
                    cout<<"sla"<<endl;
                    sla(armstand);
                    resetarm();
                    badjedone=0;
                    break;
                default:
                    break;
            }

        }
    }
   
}