File content as of revision 14:fb5d8064830d:

// EMG + KINEMATICS + PID + MOTOR CONTROL

//----------------~INITIATING-------------------------
#include "mbed.h"

// EMG                 --      DEPENDENCIES
#include <iostream>
#include "BiQuad.h"
#include "BiQuadchains_zelfbeun.h"
#include "MODSERIAL.h"

// KINEMATICS          --       DEPENDENCIES
#include "stdio.h"
#define _USE_MATH_DEFINES
#include <math.h>
#define M_PI    3.14159265358979323846 /* pi */

// PID CONTROLLER      --      DEPENDENCIES
#include "BiQuad.h"
#include "QEI.h"
//#include "HIDScope.h"


// GENERAL PIN DEFENITIONS
MODSERIAL pc(USBTX, USBRX);

// EMG     --      PIN DEFENITIONS 
DigitalOut gpo(D0);

DigitalIn button2(SW3);  
DigitalIn button1(SW2); //or SW2

DigitalOut led1(LED_GREEN);
DigitalOut led2(LED_RED);
DigitalOut led3(LED_BLUE);

//EMG tickers, these tickers are called in the mainscript with fsample 500Hz, also sends to HIDscope with same fsample
Ticker sample_ticker; //ticker for filtering pref. with 1000Hz, define in tick.attach
Ticker threshold_check_ticker;
Timer t; //timer try out for Astrid
Timer timer_calibration; //timer for EMG calibration

//Input system
AnalogIn emg1(A0); //right biceps
AnalogIn emg2(A1); //right triceps
AnalogIn emg3(A2); //left biceps
AnalogIn emg4(A3); //left triceps


// PID  CONTROLLER     --        PIN DEFENITIONS 
//AnalogIn button3(A4);
//AnalogIn button4(A5);

DigitalOut directionpin1(D7);   // motor 1
DigitalOut directionpin2(D4);   // motor 2
DigitalOut directionpin3(D13);  // motor 3
PwmOut pwmpin1(D6);             // motor 1
PwmOut pwmpin2(D5);             // motor 2
PwmOut pwmpin3(D12);            // motor 3

QEI encoder1 (D9, D8, NC, 8400, QEI::X4_ENCODING);
QEI encoder2 (D11, D10, NC, 8400, QEI::X4_ENCODING); // motor 2
QEI encoder3 (D3, D2, NC, 8400, QEI::X4_ENCODING);  // motor 3
// HIDScope scope(2);

//  PID - TICKERS
Ticker ref_rot;
Ticker show_counts;
Ticker Scope_Data;

//------------------------GLOBALS-----------------------------
// GLOBALS EMG
//Filtered EMG signals from the end of the chains
volatile double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered;
int i = 0;

//Define doubles for calibration and ticker
double ts = 0.001; //tijdsstap
double calibration_time = 55; //time EMG calibration should take

volatile double temp_highest_emg1 = 0; //highest detected value right biceps
volatile double temp_highest_emg2 = 0;
volatile double temp_highest_emg3 = 0;
volatile double temp_highest_emg4 = 0;

//Doubles for calculation threshold
double biceps_p_t = 0.4; //set threshold at percentage of highest value
double triceps_p_t = 0.5; //set threshold at percentage of highest value
volatile double threshold1;
volatile double threshold2;
volatile double threshold3;
volatile double threshold4;

// thresholdreads bools
int bicepsR;
int tricepsR;
int bicepsL;
int tricepsL;

// VARIABLES ROBOT KINEMATICS
// constants
const float la = 0.256;         // lengte actieve arm
const float lp = 0.21;          // lengte passieve arm
const float rp = 0.052;         // straal van midden end effector tot hoekpunt
const float rm = 0.23;          // straal van global midden tot motor
const float a = 0.09;           // zijde van de driehoek
const float xas = 0.40;         // afstand van motor 1 tot motor 3
const float yas = 0.346;        // afstand van xas tot motor 2
const float thetap = 0;         // rotatiehoek van de end effector


// motor locatie
const int a1x = 0;              //x locatie motor 1
const int a1y = 0;              //y locatie motor 1
const float a2x = (0.5)*xas;    // x locatie motor 2
const float a2y = yas;          // y locatie motor 2
const float a3x = xas;          // x locatie motor 3
const int a3y = 0;              // y locatie motor 3

// script voor het bepalen van de desired position aan de hand van emg (1/0)

//  EMG OUTPUT
int EMGxplus;
int EMGxmin ;
int EMGyplus;
int EMGymin ;

// Dit moet experimenteel geperfectioneerd worden
float tijdstap = 0.005;      //nu wss heel langzaam, kan miss omhoog KEER V GEEFT VERANDERING IN POSITIE
float v = 0.1;                // snelheid kan wss ook hoger

float px = 0.2;     //starting x    // BOUNDARIES
float py = 0.155;   // starting y   // BOUNDARIES

// verschil horizontale as met de actieve arm
float da1 = 1.619685; // verschil a1 hoek en motor
float da2 = -0.609780;
float da3 = 3.372859;

// limits (since no forward kinematics)
float upperxlim = 0.275; //36, 0.04, 0.315, -0.085niet helemaal naar requierments ff kijken of ie groter kan
float lowerxlim = 0.10;
float upperylim = 0.225;
float lowerylim = 0.03; //0.03 is goed

// VARIABLES PID CONTROLLER
double PI = M_PI;// CHANGE THIS INTO M_PI
double Kp = 14; //200 , 50
double Ki = 0;   //1, 0.5
double Kd = 3; //200, 10
double Ts = 0.1; // Sample time in seconds
double reference_rotation; //define as radians
double motor_position;
bool AlwaysTrue;

//----------------FUNCTIONS--------------------------

// ~~~~~~~~~~~~~~~~~~~EMG FUNCTIONS~~~~~~~~~~~~~~~~~~
void emgsample()
{
    //All EMG signal through Highpass
    double emgread1 = emg1.read();
    double emgread2 = emg2.read();
    double emgread3 = emg3.read();
    double emgread4 = emg4.read();

    double emg1_highpassed = highp1.step(emgread1);
    double emg2_highpassed = highp2.step(emgread2);
    double emg3_highpassed = highp3.step(emgread3);
    double emg4_highpassed = highp4.step(emgread4);

    //All EMG highpassed through Notch
    double emg1_notched = notch1.step(emg1_highpassed);
    double emg2_notched = notch2.step(emg2_highpassed);
    double emg3_notched = notch3.step(emg3_highpassed);
    double emg4_notched = notch4.step(emg4_highpassed);

    //All EMG notched rectify
    double emg1_abs = abs(emg1_notched);
    double emg2_abs = abs(emg2_notched);
    double emg3_abs = abs(emg3_notched);
    double emg4_abs = abs(emg4_notched);

    //All EMG abs into lowpass
    emg1_filtered = lowp1.step(emg1_abs);
    emg2_filtered = lowp2.step(emg2_abs);
    emg3_filtered = lowp3.step(emg3_abs);
    emg4_filtered = lowp4.step(emg4_abs);
    


}

void CalibrationEMG()
{
    //static float samples = calibration_time/ts;
    while(timer_calibration<55) {
        if(timer_calibration>0 && timer_calibration<10) {
            led1=!led1;
            if(emg1_filtered>temp_highest_emg1) {
                temp_highest_emg1= emg1_filtered;
                //pc.printf("Temp1 = %f \r\n",temp_highest_emg1);
            }
        }
        if(timer_calibration>10 && timer_calibration<15) {
            led1=0;
            led2=0;
            led3=0;
        }
        if(timer_calibration>15 && timer_calibration<25) {
            led2=!led2;
            if(emg2_filtered>temp_highest_emg2) {
                temp_highest_emg2= emg2_filtered;
                //pc.printf("Temp2 = %f \r\n",temp_highest_emg2);
            }
        }
        if(timer_calibration>25 && timer_calibration<30) {
            led1=0;
            led2=0;
            led3=0;
        }
        if(timer_calibration>30 && timer_calibration<40) {
            led3=!led3;
            if(emg3_filtered>temp_highest_emg3) {
                temp_highest_emg3= emg3_filtered;
                //pc.printf("Temp3 = %f \r\n",temp_highest_emg3);
            }
        }
        if(timer_calibration>40 && timer_calibration<45) {
            led1=0;
            led2=0;
            led3=0;
        }
        if(timer_calibration>45 && timer_calibration<55) {
            led2=!led2;
            led3=!led3;
            if(emg4_filtered>temp_highest_emg4) {
                temp_highest_emg4= emg4_filtered;
                //pc.printf("Temp4 = %f \r\n",temp_highest_emg4);
            }
        }
        led1=1;
        led2=1;
        led3=1;


    }

    //pc.printf("Highest value right biceps= %f \r\n", temp_highest_emg1);
    //pc.printf("Highest value right triceps= %f \r\n", temp_highest_emg2);
    //pc.printf("Highest value left biceps= %f \r\n", temp_highest_emg3);
    //pc.printf("Highest value left triceps= %f \r\n", temp_highest_emg4);


    threshold1 = temp_highest_emg1*biceps_p_t;  //Right Biceps
    threshold2 = temp_highest_emg2*triceps_p_t; //Right Triceps
    threshold3 = temp_highest_emg3*biceps_p_t;  //Left Biceps
    threshold4 = temp_highest_emg4*triceps_p_t; //Left Triceps
}

//Check if emg_filtered has reached their threshold
void threshold_check()
{

    //EMG1 threshold check
    if(emg1_filtered>threshold1) {
        bicepsR = 1;
    } else {
        bicepsR= 0;
    }
    //EMG2 threshold check
    if(emg2_filtered>threshold2) {
        tricepsR = 1;
    } else {
        tricepsR= 0;
    }
    //EMG3 threshold check
    if(emg3_filtered>threshold3) {
        bicepsL = 1;
    } else {
        bicepsL= 0;
    }
    //EMG4 threshold check
    if(emg4_filtered>threshold4) {
        tricepsL = 1;
    } else {
        tricepsL= 0;
    }

    /*
    pc.printf("Biceps Right = %i", bicepsR);
    pc.printf("Triceps Right = %i",tricepsR);
    pc.printf("Biceps Left = %i", bicepsL);
    pc.printf("Triceps Left = %i", tricepsL);
    */


}


// ~~~~~~~~~~~~~~ROBOT KINEMATICS ~~~~~~~~~~~~~~~~~~

// functie x positie
float positionx(int EMGxplus,int EMGxmin)
{
    float EMGx = EMGxplus - EMGxmin;

    float verplaatsingx = EMGx * tijdstap * v;
    float pxnieuw = px + verplaatsingx;
    // x limit
    if (pxnieuw <= upperxlim && pxnieuw >= lowerxlim) {
        px = pxnieuw;
    } else {
        if (pxnieuw >= lowerxlim) {
            px = upperxlim;
        } else {
            px = lowerxlim;
        }
    }
//printf("X eindpunt (%f) en verplaatsing: (%f)\n\r",px,verplaatsingx);
    return px;
}


// functie y positie
float positiony(int EMGyplus,int EMGymin)
{
    float EMGy = EMGyplus - EMGymin;

    float verplaatsingy = EMGy * tijdstap * v;
    float pynieuw = py + verplaatsingy;

    // y limit
    if (pynieuw <= upperylim && pynieuw >= lowerylim) {
        py = pynieuw;
    } else {
        if (pynieuw >= lowerylim) {
            py = upperylim;
        } else {
            py = lowerylim;
        }
    }
//printf("Y eindpunt (%f) en verplaatsing: (%f) \n\r",py,verplaatsingy);
    return (py);
}


//~~~~~~~~~~~~CALCULATIING MOTOR ANGLES ~~~~~~~~
// arm 1 --> reference angle motor 1
float hoek1(float px, float py) // input: ref x, ref y
{
    float c1x =  px - rp * cos(thetap +(M_PI/6));       // x locatie hoekpunt end-effector
    float c1y = py - rp*sin(thetap+(M_PI/6));           // y locatie hoekpunt end-effector
    float alpha1 = atan2((c1y-a1y),(c1x-a1x));          // hoek tussen horizontaal en lijn van motor naar bijbehorende end-effector punt
    float psi1 = acos(( pow(la,2)-pow(lp,2)+pow((c1x-a1x),2)+pow((c1y-a1y),2))/(2*la*sqrt(pow ((c1x-a1x),2)+pow((c1y-a1y),2) ))); //Hoek tussen lijn van motor naar bijbehorende end=effector punt en actieve arm
    float a1 = alpha1 + psi1 - da1;                          //hoek tussen horizontaal en actieve arm
    //printf("arm 1 = %f \n\r",a1);
    return a1;
}

// arm 2 --> reference angle motor 2
float hoek2(float px, float py)
{
    float c2x =  px - rp * cos(thetap -(M_PI/2));
    float c2y = py - rp*sin(thetap-(M_PI/2));
    float alpha2 = atan2((c2y-a2y),(c2x-a2x));
    float psi2 = acos(( pow(la,2)-pow(lp,2)+pow((c2x-a2x),2)+pow((c2y-a2y),2))/(2*la*sqrt(pow ((c2x-a2x),2)+pow((c2y-a2y),2) )));
    float a2 = alpha2 + psi2 - da2;
    //printf("arm 2 = %f \n\r",a2);
    return a2;
}

//arm 3 --> reference angle motor 3
float hoek3(float px, float py)
{
    float c3x =  px - rp * cos(thetap +(5*M_PI/6));
    float c3y = py - rp*sin(thetap+(5*M_PI/6));
    float alpha3 = atan2((c3y-a3y),(c3x-a3x));
    float psi3 = acos(( pow(la,2)-pow(lp,2)+pow((c3x-a3x),2)+pow((c3y-a3y),2))/(2*la*sqrt(pow ((c3x-a3x),2)+pow((c3y-a3y),2) )));
    float a3 = alpha3 + psi3 - da3;
    //printf("arm 3 = %f \n\r",a3);
    return a3;
}

// ~~~~~~~~~~~~~~PID CONTROLLER~~~~~~~~~~~~~~~~~~

double PID_controller(double error)
{
    static double error_integral = 0;
    static double error_prev = error; // initialization with this value only done once!
    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);

    // Proportional part:
    double u_k = Kp * error;

    // Integral part
    error_integral = error_integral + error * Ts;
    double u_i = Ki * error_integral;

    // Derivative part
    double error_derivative = (error - error_prev)/Ts;
    double filtered_error_derivative = LowPassFilter.step(error_derivative);
    double u_d = Kd * filtered_error_derivative;
    error_prev = error;

    // Sum all parts and return it
    return u_k + u_i + u_d;
}


// DIRECTON AND SPEED CONTROL
void moter_control(double u)
{

    directionpin1= u > 0.0f; //eithertrueor false
    if (fabs(u)> 0.7f) {
        u = 0.7f;
    } else {
        u= u;
    }
    pwmpin1= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value
}

void moter2_control(double u)
{
    directionpin2= u > 0.0f; //eithertrueor false
    if (fabs(u)> 0.7f) {
        u = 0.7f;
    } else {
        u= u;
    }
    pwmpin2= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value
}

void moter3_control(double u)
{
    directionpin3= u > 0.0f; //eithertrueor false
    if (fabs(u)> 0.7f) {
        u = 0.7f;
    } else {
        u= u;
    }
    pwmpin3 = fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value
}

// CONTROLLING THE MOTOR
void Motor_mover()
{
    float px = positionx(bicepsR,bicepsL);  // EMG: +x, -x
   float py = positiony(tricepsR,tricepsL);  // EMG: +y, -y
        
    double motor_position = encoder1.getPulses(); //output in counts
    double reference_rotation = hoek1(px, py);
    double error = reference_rotation - motor_position*(2*PI)/8400;
    double u = PID_controller(error);
    moter_control(u);

    double motor_position2 = encoder2.getPulses(); //output in counts
    double reference_rotation2 = hoek2(px, py);
    double error_2 = reference_rotation2 - motor_position2*(2*PI)/8400;
    double u_2 = PID_controller(error_2);
    moter2_control(u_2);

    double motor_position3 = encoder3.getPulses(); //output in counts
    double reference_rotation3 = hoek3(px, py);
    double error_3 = reference_rotation3 - motor_position3*(2*PI)/8400;
    double u_3 = PID_controller(error_3);
    moter3_control(u_3);
}

//Activate ticker for Movement state, filtering and Threshold checking
void movement_ticker_activator()
{
    sample_ticker.attach(&emgsample, ts);
    threshold_check_ticker.attach(&threshold_check, ts);
}
void movement_ticker_deactivator()
{
    sample_ticker.detach();
    threshold_check_ticker.detach();
}


//-------------------- STATE MACHINE --------------------------
enum states {MOTORS_OFF,CALIBRATION,HOMING,DEMO,MOVEMENT,CLICK};
states currentState = MOTORS_OFF; //Chosen startingposition for states
bool stateChanged = true; // Make sure the initialization of first state is executed

void ProcessStateMachine(void)
{
    switch (currentState) {
        case MOTORS_OFF:
            // Actions
            if (stateChanged) {
                // state initialization: rood
                led1 = 1;
                led2 = 0;
                led3 = 1;
                wait (1);
                stateChanged = false;
            }

            // State transition logic: Als button 1 word ingedrukt --> calibratie, anders motor uithouden
            if (!button1) {
                currentState = CALIBRATION;
                stateChanged = true;
            } else if (!button2) {
                currentState = HOMING  ;
                stateChanged = true;
            } else {
                currentState = MOTORS_OFF;
                stateChanged = true;
            }

            break;

        case CALIBRATION:
            // Actions
            if (stateChanged) {
                // state initialization: oranje
                temp_highest_emg1 = 0; //highest detected value right biceps
                temp_highest_emg2 = 0;
                temp_highest_emg3 = 0;
                temp_highest_emg4 = 0;

                timer_calibration.reset();
                timer_calibration.start();

                sample_ticker.attach(&emgsample, ts); 
                CalibrationEMG();
                sample_ticker.detach();
                timer_calibration.stop();


                stateChanged = false;
            }

            // State transition logic: automatisch terug naar motors off.

            currentState = MOTORS_OFF;
            stateChanged = true;
            break;

        case HOMING:
            // Actions
            if (stateChanged) {
                // state initialization: green
                t.reset();
                t.start();
                led1 = 0;
                led2 = 1;
                led3 = 1;
                wait (1);

                stateChanged = false;
            }

            // State transition logic: naar DEMO (button1), naar MOVEMENT(button2)
            if (!button1) {
                currentState = DEMO;
                stateChanged = true;
            } else if (!button2) {
                currentState = MOVEMENT  ;
                stateChanged = true;
            } else if (t>300) {
                t.stop();
                t.reset();
                currentState = MOTORS_OFF  ;
                stateChanged = true;
            } else {
                currentState = HOMING  ;
                stateChanged = true;
            }
            break;

        case DEMO:
            // Actions
            if (stateChanged) {
                // state initialization: light blue
                led1 = 0;
                led2 = 1;
                led3 = 0;
                wait (1);

                stateChanged = false;
            }

            // State transition logic: automatisch terug naar HOMING
            currentState = HOMING;
            stateChanged = true;
            break;

        case MOVEMENT:
            // Actions
            if (stateChanged) {
                // state initialization: purple
                //t.reset();
                //t.start();

                led1 = 1;
                led2 = 0;
                led3 = 0;
                wait(2);

                movement_ticker_activator();

                led1 = 0;
                led2 = 0;
                led3 = 0;
                wait(2);


                stateChanged = false;
            }

            // State transition logic: naar CLICK (button1), naar MOTORS_OFF(button2) anders naar MOVEMENT
            if (!button1) {
                movement_ticker_deactivator();
                currentState = CLICK;
                stateChanged = true;
            } else if (!button2) {
                movement_ticker_deactivator();
                currentState = MOTORS_OFF  ;
                stateChanged = true;
            } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0) { //this check if person is idle for more than 300seconds
                t.start();
            } else if  (bicepsR==1 || tricepsR==1 || bicepsL==1 || tricepsL==1) {
                t.stop();
                t.reset();
            }

            if(t>20) {
                movement_ticker_deactivator();
                t.stop();
                t.reset();
                currentState = HOMING  ;
                stateChanged = true;
            }
            // here ends the idle checking mode
            else{
                //For every muscle a different colour if threshold is passed
                if(bicepsR==1) {
                    led1 = 0;
                    led2 = 1;
                    led3 = 1;
                } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) {
                    led1 = 1;
                    led2 = 1;
                    led3 = 1;
                }
                if(tricepsR==1) {
                    led1 = 1;
                    led2 = 0;
                    led3 = 1;
                } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) {
                    led1 = 1;
                    led2 = 1;
                    led3 = 1;
                }
                if(bicepsL==1) {
                    led1 = 1;
                    led2 = 1;
                    led3 = 0;
                } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) {
                    led1 = 1;
                    led2 = 1;
                    led3 = 1;
                }
                if(tricepsL==1) {
                    led1 = 1;
                    led2 = 0;
                    led3 = 0;
                } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) {
                    led1 = 1;
                    led2 = 1;
                    led3 = 1;
                }
                //currentState = MOVEMENT  ;
                //stateChanged = false;
            }

            break;

        case CLICK:
            // Actions
            if (stateChanged) {
                // state initialization: blue
                led1 = 1;
                led2 = 1;
                led3 = 0;
                wait (1);

                stateChanged = false;
            }

            // State transition logic: automatisch terug naar MOVEMENT.

            currentState = MOVEMENT;
            stateChanged = true;
            break;

    }
}

// --------------------------MAIN--------------------


int main()
{
   
    //BiQuad Chain add
    highp1.add( &highp1_1 ).add( &highp1_2 );
    notch1.add( &notch1_1 ).add( &notch1_2 );
    lowp1.add( &lowp1_1 ).add(&lowp1_2);

    highp2.add( &highp2_1 ).add( &highp2_2 );
    notch2.add( &notch2_1 ).add( &notch2_2 );
    lowp2.add( &lowp2_1 ).add(&lowp2_2);

    highp3.add( &highp3_1 ).add( &highp3_2 );
    notch3.add( &notch3_1 ).add( &notch3_2 );
    lowp3.add( &lowp3_1 ).add(&lowp3_2);

    highp4.add( &highp4_1 ).add( &highp4_2 );
    notch4.add( &notch4_1 ).add( &notch4_2 );
    lowp4.add( &lowp4_1 ).add(&lowp4_2);

    pc.baud(115200);
    led1 = 1;
    led2 = 1;
    led3 = 1;
    
    pwmpin1.period_us(60); // setup motor
    ref_rot.attach(Motor_mover, 0.01f);// HAS TO GO TO STATE MACHINE
    //movement_ticker_activator();
    //emg_sample_ticker();
    while (true) {
        ProcessStateMachine();

/*      
        if (button2 == false) {
            wait(0.01f);

            // berekenen positie
            float px = positionx(1,0);  // EMG: +x, -x
            float py = positiony(0,0);  // EMG: +y, -y
            //printf("positie (%f,%f)\n\r",px,py);
        }

        if (button1 == false) {
            wait(0.01f);
            // berekenen positie
            float px = positionx(0,1);  // EMG: +x, -x
            float py = positiony(0,0);  // EMG: +y, -y
            //printf("positie (%f,%f)\n\r",px,py);
        }
/*
        if (button3 == false) {
            wait(0.01f);
            // berekenen positie
            float px = positionx(0,0);  // EMG: +x, -x
            float py = positiony(1,0);  // EMG: +y, -y
            //printf("positie (%f,%f)\n\r",px,py);
        }

        if (button4 == false) {
            wait(0.01f);
            // berekenen positie
            float px = positionx(0,0);  // EMG: +x, -x
            float py = positiony(0,1);  // EMG: +y, -y
            //printf("positie (%f,%f)\n\r",px,py);
        }
*/
    }

}