File content as of revision 13:4f4cc1a5a79a:

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


// pins
DigitalOut  motor1_direction(D4);
PwmOut      motor1_speed(D5);
DigitalOut  motor2_direction(D7);
PwmOut      motor2_speed(D6);
DigitalIn   button_1(PTC6); //counterclockwise
DigitalIn   button_2(PTA4); //clockwise
AnalogIn    PotMeter1(A4);
AnalogIn    PotMeter2(A5);
AnalogIn    EMG_bicepsright(A0);
AnalogIn    EMG_bicepsleft(A1);
AnalogIn    EMG_legright(A2);
AnalogIn    EMG_legleft(A3);
Ticker      controller;
Ticker      ticker_regelaar;
Ticker      EMG_Filter;
Ticker      Scope;
Encoder     motor1(D12,D13);
Encoder     motor2(D10,D11);
HIDScope    scope(6);
MODSERIAL   pc(USBTX, USBRX);



// Regelaar homeposition
#define SAMPLETIME_REGELAAR 0.005
volatile bool regelaar_ticker_flag;
void setregelaar_ticker_flag()
{
    regelaar_ticker_flag = true;
}

//define states
enum state {HOME, MOVE_MOTORS, BACKTOHOMEPOSITION, STOP};
uint8_t state = HOME;

// Berekening van de output shaft resolution. Het aantal counts per 1 graden verandering (PI-controller)
const double g = 360; // Aantal graden 1 rotatie
const double c = 4200; // Aantal counts 1 rotatie
const double q = c/(g);

//PI-controller constante
const double motor1_Kp = 2.0; // Dit is de proportionele gain motor 1
const double motor1_Ki = 0.002; // Integrating gain m1.
const double motor1_Ts = 0.01; // Time step m1
const double motor2_Kp = 2.0; // Dit is de proportionele gain motor 1
const double motor2_Ki = 0.002; // Integrating gain m1.
const double motor2_Ts = 0.01; // Time step m1
double err_int_m1 = 0 ; // De integrating error op het beginstijdstip m1
double err_int_m2 = 0 ; // De integrating error op het beginstijdstip m1

// Reusable P controller
double Pc1 (double error1, const double motor1_Kp)
{
    return motor1_Kp * err_int_m1;
}
double Pc2 (double error2, const double motor2_Kp)
{
    return motor2_Kp * err_int_m2;
}

// Measure the error and apply output to the plant
void motor1_controlP()
{
    double referenceP1 = PotMeter1.read();
    double positionP1 = q*motor1.getPosition();
    double motorP1 = Pc1(referenceP1 - positionP1, motor1_Kp);
}

void motor2_controlP()
{
    double referenceP2 = PotMeter2.read();
    double positionP2 = q*motor2.getPosition();
    double motorP2 = Pc2(referenceP2 - positionP2, motor2_Kp);
}

// Reusable PI controller
double PI (double error, const double Kp, const double Ki, const double Ts, double &err_int_m1)
{
    err_int_m1 = err_int_m1 * Ts*error; // Dit is de fout die er door de integrator uit wordt gehaald. Deze wordt elke meting aangepast door het &-teken
    return motor1_Kp*error + motor1_Ki*err_int_m1;
} // De totale fout die wordt hersteld met behulp van PI control.

double PI2 (double error2, const double motor2_Kp, const double motor2_Ki, const double motor2_Ts, double &err_int_m2)
{
    err_int_m2 = err_int_m2 * motor2_Ts*error2;
    return motor2_Kp*error2 + motor2_Ki*err_int_m2;
}

void motor1_controlPI()
{
    double referencePI1 = PotMeter1.read();
    double positionPI1 = q*motor1.getPosition();
    double motorPI1 = PI(referencePI1 - positionPI1, motor1_Kp, motor1_Ki, motor1_Ts, err_int_m1);
}

void motor2_controlPI()
{
    double referencePI2 = PotMeter2.read();
    double positionPI2 = q*motor2.getPosition();
    double motorPI2 = PI(referencePI2 - positionPI2, motor2_Kp, motor2_Ki, motor2_Ts, err_int_m2);
}

// Filter1 = High pass filter tot 20 Hz
double ah1_v1=0, ah1_v2=0, ah2_v1=0, ah2_v2=0;
double bh1_v1=0, bh1_v2=0, bh2_v1=0, bh2_v2=0;
double ch1_v1=0, ch1_v2=0, ch2_v1=0, ch2_v2=0;
double dh1_v1=0, dh1_v2=0, dh2_v1=0, dh2_v2=0;
const double fh1_a1=-0.50701081158, fh1_a2=0.00000000000, fh1_b0= 1, fh1_b1=-1, fh1_b2=0;
const double fh2_a1=-1.24532140600, fh2_a2=0.54379713891, fh2_b0= 1, fh2_b1=-2, fh2_b2=1;
// Filter2 = Low pass filter na 60 Hz
double al1_v1=0, al1_v2=0, al2_v1=0, al2_v2=0;
double bl1_v1=0, bl1_v2=0, bl2_v1=0, bl2_v2=0;
double cl1_v1=0, cl1_v2=0, cl2_v1=0, cl2_v2=0;
double dl1_v1=0, dl1_v2=0, dl2_v1=0, dl2_v2=0;
const double fl1_a1=0.15970686439, fl1_a2=0.00000000000, fl1_b0= 1, fl1_b1=1, fl1_b2=0;
const double fl2_a1=0.42229458338, fl2_a2=0.35581444792, fl2_b0= 1, fl2_b1=2, fl2_b2=1;
// Filter3 = Notch filter at 50 Hz
double ano1_v1=0, ano1_v2=0, ano2_v1=0, ano2_v2=0, ano3_v1=0, ano3_v2=0;
double bno1_v1=0, bno1_v2=0, bno2_v1=0, bno2_v2=0, bno3_v1=0, bno3_v2=0;
double cno1_v1=0, cno1_v2=0, cno2_v1=0, cno2_v2=0, cno3_v1=0, cno3_v2=0;
double dno1_v1=0, dno1_v2=0, dno2_v1=0, dno2_v2=0, dno3_v1=0, dno3_v2=0;
const double fno1_a1 = -0.03255814954, fno1_a2= 0.92336486961, fno1_b0= 1, fno1_b1= -0.03385540628, fno1_b2= 1;
const double fno2_a1 = 0.03447359684, fno2_a2= 0.96095720701, fno2_b0= 1, fno2_b1= -0.03385540628, fno2_b2= 1;
const double fno3_a1 =  -0.10078591122, fno3_a2= 0.96100189080, fno3_b0= 1, fno3_b1= -0.03385540628, fno3_b2= 1;
// Filter4 = Lowpass filter at 5 Hz
double alp1_v1=0, alp1_v2=0, alp2_v1=0, alp2_v2=0;
double blp1_v1=0, blp1_v2=0, blp2_v1=0, blp2_v2=0;
double clp1_v1=0, clp1_v2=0, clp2_v1=0, clp2_v2=0;
double dlp1_v1=0, dlp1_v2=0, dlp2_v1=0, dlp2_v2=0;
const double flp1_a1=-0.86962685441, flp1_a2=0.00000000000, flp1_b0= 1, flp1_b1=1, flp1_b2=0;
const double flp2_a1=-1.85211666729, flp2_a2=0.87021679713, flp2_b0= 1, flp2_b1=2, flp2_b2=1;
double y1, y2, y3, y4, y5, y6, y7, y8, y9, y10, y11, y12, y13, y14, y15, y16, y17, y18, y19, y20, y21, y22, y23, y24, y25, y26, y27, y28, y29, y30, y31, y32, y33, y34, y35, y36;
double A, B, C, D;
double final_filter1, final_filter2, final_filter3, final_filter4;

// Standaard formule voor het biquad filter
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;
    double y = b0*v + b1*v1 + b2*v2;
    v2=v1;
    v1=v;
    return y;
}

// script voor filters
void Filters()
{
    // Biceps right
    A = EMG_bicepsright.read();
    //Highpass
    y1 = biquad (A, ah1_v1, ah1_v2, fh1_a1, fh1_a2, fh1_b0*0.753507, fh1_b1*0.753507, fh1_b2*0.753507);
    y2 = biquad (y1, ah2_v1, ah2_v2, fh2_a1, fh2_a2, fh2_b0*0.697278, fh2_b1*0.697278, fh2_b2*0.697278);
    //Lowpass
    y3 = biquad (y2, al1_v1, al1_v2, fl1_a1, fl1_a2, fl1_b0*0.579853, fl1_b1*0.579853, fl1_b2*0.579853);
    y4 = biquad (y3, al2_v1, al2_v2, fl2_a1, fl2_a2, fl2_b0*0.444527, fl2_b1*0.444527, fl2_b2*0.444527);
    // Notch
    y5 = biquad (y4, ano1_v1, ano1_v2, fno1_a1, fno1_a2, fno1_b0*0.968276, fno1_b1*0.968276, fno1_b2*0.968276);
    y6 = biquad (y5, ano2_v1, ano2_v2, fno2_a1, fno2_a2, fno2_b0*0.953678, fno2_b1*0.953678, fno2_b2*0.953678);
    y7 = biquad (y6, ano3_v1, ano3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2);
    // Rectify sample
    y8 = fabs(y7);
    // Make it smooth
    y9 = biquad (y8, alp1_v1, alp1_v2, flp1_a1, flp1_a2, flp1_b0*0.065187, flp1_b1* 0.065187, flp1_b2* 0.065187);
    final_filter1 = biquad(y9, alp2_v1, alp2_v2, flp2_a1, flp2_a2, flp2_b0*0.004525, flp2_b1*0.004525, flp2_b2*0.004525);

    //Biceps left
    B = EMG_bicepsleft.read();
    //Highpass
    y10 = biquad (B, bh1_v1, bh1_v2, fh1_a1, fh1_a2, fh1_b0*0.753507, fh1_b1*0.753507, fh1_b2*0.753507);
    y11 = biquad (y10, bh2_v1, bh2_v2, fh2_a1, fh2_a2, fh2_b0*0.697278, fh2_b1*0.697278, fh2_b2*0.697278);
    //Lowpass
    y12 = biquad (y11, bl1_v1, bl1_v2, fl1_a1, fl1_a2, fl1_b0*0.579853, fl1_b1*0.579853, fl1_b2*0.579853);
    y13 = biquad (y12, bl2_v1, bl2_v2, fl2_a1, fl2_a2, fl2_b0*0.444527, fl2_b1*0.444527, fl2_b2*0.444527);
    // Notch
    y14 = biquad (y13, bno1_v1, bno1_v2, fno1_a1, fno1_a2, fno1_b0*0.968276, fno1_b1*0.968276, fno1_b2*0.968276);
    y15 = biquad (y14, bno2_v1, bno2_v2, fno2_a1, fno2_a2, fno2_b0*0.953678, fno2_b1*0.953678, fno2_b2*0.953678);
    y16 = biquad (y15, bno3_v1, bno3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2);
    // Rectify sample
    y17 = fabs(y16);
    // Make it smooth
    y18 = biquad (y17, blp1_v1, blp1_v2, flp1_a1, flp1_a2, flp1_b0*0.065187, flp1_b1*0.065187, flp1_b2*0.065187);
    final_filter2 = biquad(y18, blp2_v1, blp2_v2, flp2_a1, flp2_a2, flp2_b0*0.004525, flp2_b1*0.004525, flp2_b2*0.004525);

    /// EMG Filter right leg
    C = EMG_legright.read();
    //Highpass
    y19 = biquad (C, ch1_v1, ch1_v2, fh1_a1, fh1_a2, fh1_b0*0.753507, fh1_b1*0.753507, fh1_b2*0.753507);
    y20 = biquad (y19, ch2_v1, ch2_v2, fh2_a1, fh2_a2, fh2_b0*0.697278, fh2_b1*0.697278, fh2_b2*0.697278);
    //Lowpass
    y21 = biquad (y20, cl1_v1, cl1_v2, fl1_a1, fl1_a2, fl1_b0*0.579853, fl1_b1*0.579853, fl1_b2*0.579853);
    y22 = biquad (y21, cl2_v1, cl2_v2, fl2_a1, fl2_a2, fl2_b0*0.444527, fl2_b1*0.444527, fl2_b2*0.444527);
    // Notch
    y23 = biquad (y22, cno1_v1, cno1_v2, fno1_a1, fno1_a2, fno1_b0*0.968276, fno1_b1*0.968276, fno1_b2*0.968276);
    y24 = biquad (y23, cno2_v1, cno2_v2, fno2_a1, fno2_a2, fno2_b0*0.953678, fno2_b1*0.953678, fno2_b2*0.953678);
    y25 = biquad (y24, cno3_v1, cno3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2);
    // Rectify sample
    y26 = fabs(y25);
    // Make it smooth
    y27 = biquad (y26, clp1_v1, clp1_v2, flp1_a1, flp1_a2, flp1_b0*0.065187, flp1_b1*0.065187, flp1_b2*0.065187);
    final_filter3 = biquad(y27, clp2_v1, clp2_v2, flp2_a1, flp2_a2, flp2_b0*0.004525, flp2_b1*0.004525, flp2_b2*0.004525);

    // EMG filter left leg
    D = EMG_legleft.read();
    //Highpass
    y28 = biquad (D, dh1_v1, dh1_v2, fh1_a1, fh1_a2, fh1_b0*0.753507, fh1_b1*0.753507, fh1_b2*0.753507);
    y29 = biquad (y28, dh2_v1, dh2_v2, fh2_a1, fh2_a2, fh2_b0*0.697278, fh2_b1*0.697278, fh2_b2*0.697278);
    //Lowpass
    y30 = biquad (y29, dl1_v1, dl1_v2, fl1_a1, fl1_a2, fl1_b0*0.579853, fl1_b1*0.579853, fl1_b2*0.579853);
    y31 = biquad (y30, dl2_v1, dl2_v2, fl2_a1, fl2_a2, fl2_b0*0.444527, fl2_b1*0.444527, fl2_b2*0.444527);
    // Notch
    y32 = biquad (y31, dno1_v1, dno1_v2, fno1_a1, fno1_a2, fno1_b0*0.968276, fno1_b1*0.968276, fno1_b2*0.968276);
    y33 = biquad (y32, dno2_v1, dno2_v2, fno2_a1, fno2_a2, fno2_b0*0.953678, fno2_b1*0.953678, fno2_b2*0.953678);
    y34 = biquad (y33, dno3_v1, dno3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2);
    // Rectify sample
    y35 = fabs(y34);
    // Make it smooth
    y36 = biquad (y35, dlp1_v1, dlp1_v2, flp1_a1, flp1_a2, flp1_b0*0.065187, flp1_b1*0.065187, flp1_b2*0.065187);
    final_filter4 = biquad(y36, dlp2_v1, dlp2_v2, flp2_a1, flp2_a2, flp2_b0*0.004525, flp2_b1*0.004525, flp2_b2*0.004525);
}


const int pressed = 0;

// constantes voor berekening homepositie
double H1;
double H2;
double P1;
double P2;
// Safety stop. Motoren mogen niet verder dan 90 graden bewegen.
volatile bool safety_stop;

void move_motor1()
{
    if (safety_stop == true && (final_filter1 > 0.02 || button_1 == pressed)) {
        pc.printf("motor1 cw \n\r");
        motor1_direction = 0; //counterclockwise
        motor1_speed = 0.1;
    } else if (safety_stop == true && (final_filter2 > 0.02 || button_2 == pressed)) {
        pc.printf("motor1 ccw \n\r");
        motor1_direction = 1; //clockwise
        motor1_speed = 0.1  ;
    } else {
        pc.printf("Not moving1 \n\r");
        motor1_speed = 0;
    }
}

void move_motor2()
{
    if (safety_stop == true && (final_filter3 > 0.08 || button_1 == pressed)) {
        pc.printf("motor2 cw \n\r");
        motor2_direction = 1; //clockwise
        motor2_speed = 0.4;
    } else if (safety_stop == true && (final_filter4 > 0.08 || button_2 == pressed)) {
        pc.printf("motor2 ccw \n\r");
        motor2_direction = 0; //counterclockwise
        motor2_speed = 0.4;
        }
         else if (P2 > 500) {
        safety_stop = false;
        pc.printf("Stop! /n/r");
        motor2_direction = 1;
        motor2_speed = 0.1;
        wait(0.2);
        safety_stop = true;
        pc.printf("En door /n/r");
    } else if (P2 < -500) {
        safety_stop = false;
        pc.printf("Stop! /n/r");
        motor2_direction = 0;
        motor2_speed = 0.1;
        wait(0.2);
        safety_stop = true;
        pc.printf("En gaan /n/r");
    } else {
        pc.printf("Not moving2 \n\r");
        motor2_speed = 0;
    }
   
}

void movetohome()
{
    P1 = motor1.getPosition();
    P2 = motor2.getPosition();

    if ((P1 >= -28 && P1 <= 28) || (P2 >= -28 && P2 <= 28)) {
        motor1_speed = 0;
    } else if (P1 > 24) {
        motor1_direction = 1;
        motor1_speed = 0.1;
    } else if (P1 < -24) {
        motor1_direction = 0;
        motor1_speed = 0.1;
    } else if (P2 > 24) {
        motor2_direction = 1;
        motor2_speed = 0.1;
    } else if (P2 < -24) {
        motor2_direction = 0;
        motor2_speed = 0.1;
    }
}

void HIDScope ()
{
    scope.set (0, final_filter1);
    scope.set (1, final_filter2);
    scope.set (2, final_filter3);
    scope.set (3, final_filter4);
    scope.set (4, motor1.getPosition());
    scope.set (5, motor2.getPosition());
    scope.send ();
}

int main()
{
    safety_stop = true;
    while (true) {
        pc.baud(9600);                          //pc baud rate van de computer
        EMG_Filter.attach_us(Filters, 5e4);     //filters uitvoeren
        Scope.attach_us(HIDScope, 5e4);         //EMG en encoder signaal naar de hidscope sturen

        switch (state) {                            //zorgt voor het in goede volgorde uitvoeren van de cases

            case HOME: {    //positie op 0 zetten voor arm 1
                pc.printf("home\n\r");
                H1 = motor1.getPosition();
                H2 = motor2.getPosition();
                pc.printf("Home-position is %f\n\r", H1);
                pc.printf("Home-pso is %f\n\r", H2);
                state = MOVE_MOTORS;
                wait(2);
                break;
            }

            case MOVE_MOTORS: {          //motor kan cw en ccw bewegen a.d.h.v. buttons
                pc.printf("move_motor\n\r");
                while (state == MOVE_MOTORS) {
                    move_motor1();
                    move_motor2();
                    if (button_1 == pressed && button_2 == pressed) {
                        motor1_speed = 0;
                        motor2_speed = 0;
                        state = BACKTOHOMEPOSITION;
                    }
                }
                wait (1);
                break;
            }


            case BACKTOHOMEPOSITION: {  //motor gaat terug naar homeposition
                pc.printf("backhomeposition\n\r");
                wait (1);

                ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR);
                while(state == BACKTOHOMEPOSITION) {
                    movetohome();
                    while(regelaar_ticker_flag != true);
                    regelaar_ticker_flag = false;

                    pc.printf("motor1 pos %d, motor2 pos %d", motor1.getPosition(), motor2.getPosition());
                    pc.printf("referentie %f, %f \n\r", H1, H2);

                    if (P1 <=24 && P1 >= -24 && P2 <=24 && P2 >= -24) {
                        pc.printf("motor1 pos %d", motor1.getPosition());
                        pc.printf("motor2 pos %d", motor2.getPosition());
                        pc.printf("referentie %f %f\n\r", H1, H2);
                        state = STOP;
                        pc.printf("Laatste positie %d %d\n\r", motor1.getPosition(),motor2.getPosition());
                        break;
                    }
                }
            }
            case STOP: {
                static int c;
                while(state == STOP) {
                    motor1_speed = 0;
                    motor2_speed = 0;
                    if (c++ == 0) {
                        pc.printf("einde\n\r");
                    }
                }
                break;
            }
        }
    }
}