File content as of revision 2:d04df4be6cf7:

#include "mbed.h"
#include "encoder.h"
#include "MODSERIAL.h"
#include "math.h"
#include "HIDScope.h"
#include "biquadFilter.h"
#include "iostream"
#define M_PI           3.14159265358979323846

DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield)
PwmOut motor2speed(D5);
DigitalOut motor1direction(D7); //D6 en D7 voor motor 1 (op het motorshield)
PwmOut motor1speed(D6);
AnalogIn potmeter1(A3);         //blauw draaiknopje, nr1
AnalogIn potmeter2(A2);         //nr 2
Encoder encoder2(D13,D12,true);
Encoder encoder1(D11,D10,true);
MODSERIAL pc(USBTX,USBRX);

Ticker myControllerTicker1, myControllerTicker2, countStepTickerX, countStepTickerY, buttonStateCheckX, buttonStateCheckY;
bool motor1_PID_Controller_go = false;
bool countStepX_go = false;
bool countStepY_go = false;
bool checkButtonStateX_go = false;
bool checkButtonStateY_go = false;
bool Filter_go = false;

//DigitalIn telknopX(SW2);
//DigitalIn telknopY(SW3);
DigitalOut ledX(D2);
DigitalOut ledY(D3);

double m1_Kp = 0.0003, m1_Ki = 0.0, m1_Kd = 0.0;
const double m1_Ts = 0.01;
double m1_err_int = 0, m1_prev_err = 0;
double m1_u_prev = 0;

/*
Gain = 0.002356
B = [ 1.00000000000,  2.00000000000,  1.00000000000]
A = [ 1.00000000000, -1.85808607200,  0.86750940236]
*/

const double m1_f_a1 = -1.94042975320, m1_f_a2 = 0.94215346226, m1_f_b0 = 1.0*0.000431, m1_f_b1 = 2.0*0.000431, m1_f_b2 = 1.0*0.000431; 
                                    //dit zijn de constanten die bij de filter horen (de a0 t/m b2) a0 is altijd 1
double m1_f_v1 = 0, m1_f_v2 = 0;    //de variabelen van de filter 

int reference1;
int reference2;
int difference1;
int difference2;
int buttonStateX = 1;
int buttonStateY = 1;
int motorDirectionX = 1;
int motorDirectionY = 1;
int n;
int countsX=0;
int countsY=0;
int countStateX = 1;
int countStateY = 1;

float x = 4.0;      // voorbeeld, moet eigenlijk bepaald worden door EMG signaal
float y = 4.0;      // is het 4de vakje van rechtsonderin de hoek geteld
float xdist_from_board = 4.0;   //er moet 2 van afgetrokken worden. Als de as 6cm van het bord staat moet er 4 worden ingevuld. Telt namelijk
float ydist_from_board = 4.0;   //vanaf het midden van een vakje.
double constante1;
double constante2;
double constante3;

// --- EMG declaraties --- //
AnalogIn    emg1(A0);
AnalogIn    emg2(A1);
DigitalIn button(SW2);
DigitalOut led_green(LED_GREEN);
DigitalOut led_red(LED_RED);
DigitalOut led_blue(LED_BLUE);

double filtered_notch1;
double filtered_notch2;
double signal1;
double signal2;
bool start = false;

HIDScope scope(2); //4 channels
Ticker      scopeTimer;
Ticker      Amplitude_height;
int counter = 0;
using namespace std;

double high[2] = {};
double low[2] = {};
double notch[2] = {};
double max1 =0.01;
double max2=0.01;

// ----- Filters ----- //

//tweede highpass filter, poging met latere cutoff frequency: werkt niet. Nu alleen nog bewegingsartefacten te zien, geen kracht!
//9 oktober: nu werkt het wel. Een beetje beweging te zien, maar aanspannen geeft hogere amplitude
biquadFilter     High_1_Filter1(  -0.69832496500, 0.00000000000, 0.849163, -0.849163, 0.00000000000);
biquadFilter     High_2_Filter1( -1.87472749640, 0.90756583051, 0.945573, 1.891146, 0.945573);
biquadFilter     High_1_Filter2(  -0.69832496500, 0.00000000000, 0.849163, -0.849163, 0.00000000000);
biquadFilter     High_2_Filter2( -1.87472749640, 0.90756583051, 0.945573, 1.891146, 0.945573);
//Notch filter voor 50 Hz. Butter
biquadFilter     Notch_Filter1(-1.65238019196, 0.73741535193, 0.86870768, -1.652380199639, 0.86870768);
biquadFilter     Notch_Filter2(-1.65238019196, 0.73741535193, 0.86870768, -1.652380199639, 0.86870768);
//Lowpass cheby II. bestaat uit twee biquads. De B coefficienten moeten maal de gain gedaan worden.
biquadFilter     Low_1_Filter1(  -0.96240334975 , 0.00000000000 , 0.018798 , 0.018798 , 0.000000 );  // a1, a2, b0, b1, b2.
biquadFilter     Low_2_Filter1( -1.96214371581 , 0.96354072388 , 0.030252 , -0.0591069922, 0.030252 );
biquadFilter     Low_1_Filter2(  -0.96240334975 , 0.00000000000 , 0.018798 , 0.018798 , 0.000000 );  // a1, a2, b0, b1, b2.
biquadFilter     Low_2_Filter2( -1.96214371581 , 0.96354072388 , 0.030252 , -0.0591069922, 0.030252 );

double* LowFilter(double n3_emg1, double n3_emg2)
{
    double l1_emg1 = Low_1_Filter1.step(n3_emg1);
    double l1_emg2 = Low_1_Filter2.step(n3_emg2);
    low[0] = Low_2_Filter1.step(l1_emg1);
    low[1] = Low_2_Filter2.step(l1_emg2);
    return low;
}

double* HighFilter(void)
{
    double reademg1 = emg1.read();
    double reademg2 = emg2.read();
    scope.set(0, reademg1);
    scope.set(1, reademg2);
    scope.send();
    double h1_emg1 = High_1_Filter1.step(reademg1);
    high[0] = High_2_Filter1.step(h1_emg1);
    double h1_emg2 = High_1_Filter2.step(reademg2);
    high[1] = High_2_Filter2.step(h1_emg2);
    return high;
}

double* NotchFilter(double n_emg1, double n_emg2  )
{
    notch[0] = Notch_Filter1.step(n_emg1);
    notch[1] = Notch_Filter2.step(n_emg2);
    return notch;
}

void Filter(void)
{
    HighFilter();
    double h2_emg1_abs = fabs(high[0]);
    double h2_emg2_abs = fabs(high[1]);
    LowFilter(h2_emg1_abs, h2_emg2_abs);
    double filtered_emg1 = low[0];
    double filtered_emg2 = low[1];
    NotchFilter(filtered_emg1, filtered_emg2);
    filtered_notch1 = notch[0];
    filtered_notch2 = notch[1];
    signal1 = filtered_notch1/max1;
    signal2 = filtered_notch2/max2;
    //scope.set(2, filtered_notch1);
    //scope.set(3, filtered_notch2);
    //scope.set(0, reademg1);
    //scope.set(1, reademg2);
    scope.send();
}



// ------ OVERIGE FUNCTIES ------ //

bool buttonReleasedX(DigitalIn& button)
{
    if (button.read() == 1 && buttonStateX == 0) {
        buttonStateX = 1;
        return true;
    }
    buttonStateX = button.read();
    return false;
}

bool buttonReleasedY(DigitalIn& button)
{
    if (button.read() == 1 && buttonStateY == 0) {
        buttonStateY = 1;
        return true;
    }
    buttonStateY = button.read();
    return false;
}

bool buttonHold(DigitalIn& button)
{
    return (button.read() == 0);
}

float bereken_hoek1(float x, float y)
{
    float langste_zijde = sqrt(pow(x*4,2) + pow(y*4,2));    //pythagoras
    float alpha = acos((langste_zijde/2)/35);               //4200=1 rondje
    return (atan((4*y)/(4*x)) + alpha)*4200/(2*M_PI);       // omrekenen van radialen naar counts = *4200/(2*M_PI), dit moet nog omgerekend worden met de gear ratio van de extra tandwielen.
}

float bereken_hoek2(float x, float y)
{
    float langste_zijde = sqrt(pow(x*4,2) + pow(y*4,2));    //pythagoras
    float alpha = acos((langste_zijde/2)/35);
    float beta = 0.5*M_PI - alpha;
    return (2*beta)*4200/(2*M_PI);                          // x wordt hoek voor eerste arm t.o.v. x-as (CW), y hoek voor tweede arm t.o.v. eerste arm (CW)
}

void Amplitude() //true is aangespanen, false is niet aangespannen. met 2 thresholds. Dit moet nog in procent worden omgezet!
{
    if(start == false && signal1< 0.6) {   //moeten waarden 70 en 20 worden als drempelwaarden
        start = true;
        //printf("start 0 \n\r");
    }
    if (start == true && signal1<0.6) {
        start = true;
        //printf("start 1\n\r");
    }
    if (start == true && signal1< 0.2) {
        start = false;
        //printf("start 1\n\r");
    }
    if (start == false && signal1 <0.2) {
        start = false;
        //printf("start 0\n\r");
    }
    if (start == true && signal1 <0.6 && signal1>0.2) {
        start = true;
        //printf("start 1\n\r");
    }
    if (start == false && signal1 <0.6 && signal1>0.2) {
        start = false;
        //printf("start 0\n\r");
    }
}

// ----- KALIBRATIE ------ //

void MaxValue()
{
    int length = 500;
    double meetwaarden1 [500] = {};             //lege array maken voor emg1
    double meetwaarden2 [500] = {};             //lege array maken voor emg2
    for(int i = 0; i<500; i++) {                //array vullen met meetwaarden van emg1
        meetwaarden1[i] = filtered_notch1;
        wait(0.005);                             // zonder wait zit er maar een dezelfde waarde in de array! eventueel een ticker er van maken
    }
    for(int i = 0; i<500; i++) {                //array vullen met meetwaarden van emg2
        meetwaarden2[i] = filtered_notch2;
        wait(0.005);
    }
    //printarray(meetwaarden, 500);             //was als controle
    for(int i = 0; i<length; i++) {             //maximale waarde van de array berekenen
        if(meetwaarden1[i] > max1) {
            max1= meetwaarden1[i];
        }
    }
    for(int i = 0; i<length; i++) {             //maximale waarde van de array berekenen
        if(meetwaarden2[i] > max2) {
            max2 = meetwaarden2[i];
        }
    }
    printf("max value of emg1 is %f, and of emg 2 is %f \n\r", max1, max2);
}

void Calibration()
{
    switch(button.read()) {
        case 0:                         //knop wel indrukken
            switch(counter) {
                case 0:             //eerste keer knop indrukken
                    led_red.write(0);           //rode led aan
                    MaxValue();
                    led_green.write(0);         //groene led aan + rode = geel
                    wait(1);      //even wachten, zodat er tijd is om de knop weer los te laten
                    counter++;
                    break;
                case 1:              //tweede keer knop indrukken
                    led_red.write(1);            //uit
                    led_green.write(1);          //uit
                    led_blue.write(0);           //blauw ledje aan

                    // move arm to 0 position: motoren aansturen.
                    //emg_signal1 = filtered_notch/max*100 //schalen
                    wait(1);
                    counter++;
                    break;
                case 2:
                    //encoder....setPosition(0);
                    led_blue.write(1);      //uit
                    led_green.write(0);      // ledje groen aan
                    wait(1);
                    counter++;
                    break;
            }
            break;
    }
}

//voor de motoren: als counter =0: motorspeed =0
//als counter = 1: niet in vakjes rekenen
// als counter >1: wel in vakjes rekenen


//-------------------------- MOTORCONTROLLERS --------------------------//

void constanteBepalen()
{
    constante1 = potmeter1.read();
    constante2 = potmeter2.read();
    m1_Kd = constante2*0.001;                     //loopt nu van 0 tot 1
    m1_Ki = constante1*0.001;
}

double biquad(double u, double &v1, double &v2, const double a1, const double a2,
    const double b0, const double b1, const double b2){
        double v = u-a1*v1-a2*v2;       //formule uit slides van biquad filter
        double y = b0*v+b1*v1+b2*v2;    //zie ook slides
        v2 = v1; v1 =v;
        return y;
}

double PID(double e, const double Kp, const double Ki, const double Kd, double Ts, 
    double &e_int, double &e_prev, double &f_v1, double &f_v2, const double f_a1,
    const double f_a2, const double f_b0, const double f_b1, const double f_b2){    //In de slides staat f_b3, maar volgens mij moet het f_b2 zijn
    //Bereken eerst afgeleide van de error
    double e_der = (e-e_prev)/Ts;                                       
    double e_der2 = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);    //de y die de biquad returnt is de gefilterde e_der
    e_prev = e;
    //Bereken nu de geintegreerde versie van de error
    e_int = e_int + Ts*e;
    //PID
    
    return Kp*e+Ki*e_int+Kd*e_der2;  //formule uit slides 
} 

// ---- PID Controller ----- //

void motor1_PID_Controller()
{
    //reference1 = potmeter1.read()*4200;                     //draaiknop uitlezen, tussen 0 en 1
    float hoek1 = bereken_hoek1(countsX,countsY);
    reference1=hoek1;
    double position =(encoder1.getPosition());              // waarde tussen 0 en 4200
    
    scope.set(0,reference1);
    scope.set(1,position);
    scope.send();
    
    difference1 = reference1 - position;
    constanteBepalen();
    double u = PID( reference1 - position, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int,m1_prev_err, m1_f_v1, m1_f_v2, m1_f_a1, m1_f_a2, m1_f_b0, m1_f_b1, m1_f_b2 );
    if (u < 0) {
        motor1direction = 1;                            //directie bepalen. Waardes zijn tegenovergesteld vergeleken met motor 2
    } else if (u>=0) {
        motor1direction = 0;
    }
    if(counter != 0) {
        motor1speed = fabs(u);
    }
    pc.printf("u = %.2f \r\n", u); 
    
}

//-------------------------- POSITIEBEPALING --------------------------//
/*
void checkButtonStateX()
{
    if (buttonReleasedX(telknopX)) {
        countStateX = -countStateX;
    }
}

void checkButtonStateY()
{
    if (buttonReleasedY(telknopY)) {
        countStateY = -countStateY;
    }
}
*/
void countStepX()
{
    if (start == true) {
        countsX+= countStateX;             //hier moet de richting dus veranderen
        if(countsX >= 10) {
            countsX = 10;
        } else if(countsX<=0) {
            countsX = 0;
        }
    }
}

void countStepY()
{
    if (start == true) {          //X en Y kunnen niet tegelijk optellen, alleen als de ander niet ingedrukt is // start moet nog start1 en start2 worden
        countsY+= countStateY;
        if(countsY >= 10) {
            countsY = 10;
        } else if(countsY<=0) {
            countsY = 0;
        }
    }
}


void motor1_PID_Controller_activate(){ 
    motor1_PID_Controller_go = true; 
}

/*void countStepX_activate(){
    countStepX_go = true; 
}
*/
void checkButtonState_activate(){
    checkButtonStateX_go = true; 
    checkButtonStateY_go = true;
}
/*
void countStepY_activate(){
    countStepY_go = true; 
}


void checkButtonStateY_activate(){
    checkButtonStateY_go = true;
}
*/
void Filter_activate(){
    Filter_go = true;
}

int main()
{
    pc.baud(9600);
    encoder1.setPosition(0);
    encoder2.setPosition(0);
    
    myControllerTicker1.attach( &motor1_PID_Controller_activate, 0.01f);   // Function@ hz
    //countStepTickerX.attach( &countStepX_activate, 1.0f);                 // Function@ hz
    //countStepTickerY.attach( &countStepY_activate, 1.04f);                 // Function@ hz
    //buttonStateCheckX.attach( &checkButtonStateX_activate, 0.001f);         // Function@ hz
    //buttonStateCheckY.attach( &checkButtonStateY_activate, 0.001f);            // Function@ hz
    //buttonStateCheck.attach( &checkButtonState_activate,0.001f);

//EMG-gedeelte
    led_green.write(1);
    led_red.write(1);
    led_blue.write(1);
    scopeTimer.attach(Filter_activate, 0.001);
    //scopeTimer.attach(Filter, 0.001);
    //Amplitude_height.attach(Amplitude, 0.1);   //elke 0.05 seconde start is 0 of 1 printen


    while (true) {

        pc.printf(" 1: pos: %d, ref: %d, dif: %d, Ki: %.5f, Kd: %.5f, countsX: %d, countsY: %d \r\n", encoder1.getPosition(), reference1,difference1,m1_Ki,m1_Kd,countsX,countsY);
        //pc.printf(" 2: pos: %d, ref: %d, dif: %d \r\n", encoder2.getPosition(), reference2,difference2);
        
        Calibration();
        
        if(motor1_PID_Controller_go) { 
            motor1_PID_Controller_go = false;
            motor1_PID_Controller();
        }
        /*
        if(countStepX_go) {
            countStepX_go = false;
            countStepX();
        }
        if(countStepY_go) {
            countStepY_go = false;
            countStepY();
        }
        
        if(checkButtonStateX_go) {
            checkButtonStateX_go = false;
            checkButtonStateX();
        }
        if(checkButtonStateY_go) {
            checkButtonStateY_go = false;
            checkButtonStateY();
        }
        */
        if(Filter_go) {
            Filter_go = false;
            Filter();
        }
    }
}