File content as of revision 18:60251896da8d:

#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    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 = g/c;

// 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 P1 = q*motor1.getPosition();
double P2 = 2.166*q*motor2.getPosition();
// Safety stop. Motoren mogen niet verder dan 90 graden bewegen.
volatile bool safety_stop;

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

void move_motor2()
{
    if (safety_stop == true && (final_filter3 > 0.04 && final_filter1 < 0.02 && final_filter2 < 0.02 && final_filter4 < 0.02)) {
        pc.printf("motor2 cw \n\r");
        motor2_direction = 1; //clockwise
        motor2_speed = 0.4;
    } else if (button_2 == pressed || (final_filter4 > 0.03 && final_filter1 < 0.02 && final_filter2 < 0.02 && final_filter3 < 0.02)) {
        pc.printf("motor2 ccw \n\r");
        motor2_direction = 0; //counterclockwise
        motor2_speed = 0.08;
        }
         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 movetohome1()
{
   double Q1 = motor1.getPosition();

    if ((Q1 > -13 && Q1 < 13)) {
        motor1_speed = 0;
    } else if (Q1 > 12) {
        motor1_direction = 1;
        motor1_speed = 0.1;
    } else if (Q1 < -12) {
        motor1_direction = 0;
        motor1_speed = 0.1;
    } 
}

void movetohome2()
{
  double Q2 = motor2.getPosition();

    if (Q2 > -26 && Q2 < 26){
        motor2_speed = 0;
    }else if (Q2 > 25) {
        motor2_direction = 0;
        motor2_speed = 0.1;
    } else if (Q2 < -12) {
        motor2_direction = 1;
        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, P1);
    scope.set (5, P2);
    scope.send ();
}

int main()
{
    safety_stop = true;
    while (true) {
        pc.baud(9600);                          //pc baud rate van de computer
        EMG_Filter.attach_us(Filters, 5e3);     //filters uitvoeren
        Scope.attach_us(HIDScope, 5e3);         //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");
                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) {
                    movetohome1();
                    movetohome2();
                    while(regelaar_ticker_flag != true);
                    regelaar_ticker_flag = false;

                    pc.printf("motor1 pos %d, motor2 pos %d", P1, P2);

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