File content as of revision 3:8eb1b9e0d676:

#include <math.h>
#include "mbed.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "FastPWM.h"
#include "HIDScope.h"
#include <stdio.h>      /* printf */
#include <stdlib.h>     /* abs */
#include <complex>
#include "BiQuad.h"
 
 
const double PI = 3.1415926535897;
 
InterruptIn but1(D1); 
InterruptIn but2(D0); 
DigitalIn butsw2(SW3); 
DigitalIn butsw3(SW2); 
AnalogIn potMeter1(A5);
AnalogIn potMeter2(A4);
 
DigitalOut motor1_direction(D4);                                                //richting van motor1
FastPWM motor1_pwm(D5);                                                         //Motor 1 PWM controle van de snelheid
DigitalOut motor2_direction(D7);                                                //richting van motor2
FastPWM motor2_pwm(D6);                                                         //Motor 2 PWM controle van de snelheid
 
Ticker loop_ticker;
// SERIAL COMMUNICATION WITH PC.................................................
Serial pc(USBTX, USBRX);
 
enum States {s_idle, s_calibratie_encoder, s_demonstratie, s_EMGcalibratie, s_EMG};
States state;
 
// ENCODER .....................................................................
QEI enc1 (D13, D12, NC, 4200); //encoder 1 gebruiken
QEI enc2 (D9,  D8,  NC, 4200); //encoder 2 gebruiken
 
const float dt = 0.001;
double e_int = 0;
double e_int2 = 0;
double theta1;
double theta2;
int enc1_zero = 0;
int enc2_zero = 0;
int enc1_value;
int enc2_value;
const int maxwaarde = 400;
 
volatile double Huidigepositie1=0;
volatile double Huidigepositie2=0;
volatile double motorValue1;
volatile double motorValue2;
volatile double refP;
volatile double refP2;
volatile double error1;
volatile double error2;
 
bool state_changed = false;
volatile bool A=true;
volatile bool B=true;
volatile bool but1_pressed = false;
volatile bool but2_pressed = false;
volatile bool butsw2_pressed = false;
volatile bool butsw3_pressed = false;
volatile bool failure_occurred = false;
 
const bool clockwise1 = true;
const bool clockwise2 = true;
volatile bool direction1 = clockwise1;
volatile bool direction2 = clockwise2;
 
// RKI VARIABELEN...............................................................
// Lengtes zijn in meters
// Vaste variabelen:
const double L0 = 0.25;                                                         // lengte arm 1 --> moet nog goed worden opgemeten!
const double L2 = 0.375;                                                         // Lengte arm 2 --> moet ook nog goed opgemeten worden!
const double r_trans = 0.035;                                                   // gebruiken voor translation naar shaft rotation
//const double gear1=1.0;
const double gear2=1.25;


// Variërende variabelen:
double q1 = 0;                                                                  // Motorhoek van joint 1 op beginpositie
double q2 = PI/2;                                                               // Motorhoek van joint 2 op beginpositie
double v_x;                                                                     // Snelheid van end effector in x richting --> Door EMG signals
double v_y;                                                                     // Snelheid van end effector in y richting --> Door EMG signals
 
double Lq1;                                                                     // Translatieafstand als gevolg van motor rotation joint 1
double Cq2;                                                                     // Joint angle van het systeem (gecorrigeerd voor gear ratio 1:1.1)
 
double q1_dot;                                                                  // Benodigde hoeksnelheid van motor 1
double q2_dot;                                                                  // Benodigde hoeksnelheid van motor 2
 
double q1_ii;                                                                   // Referentie signaal voor motorhoek q1
double q2_ii;                                                                   // Referentie signaal voor motorhoek q2
 
//PI VARIABELEN................................................................
const double kp=0.01;                                                          // Soort van geschaald --> meer onderzoek nodig
const double ki=0.0001;
const double kp2=0.01;
const double ki2=0.0001;


double maxPWM1 = 0.2;
double maxPWM2 = 0.2;
 
//FILTERS EMG...................................................................
 
//Filters EMG
//Filters Linker Biceps
//In de volgorde High pass, notch, low pass
//BiQuad LBHP1(1, 2.000000023182220, 0.999999981993818, 1.893415601022358, 0.896255659548714);
//BiQuad LBHP2(1, 1.999999976817780, 0.999999976817780, 1.778313488139574, 0.790599715524785);
BiQuad LBHP1(0.995566972017647,  -1.991133944035294,   0.995566972017647, 1.000000000000000,  -1.991114292201654,   0.991153595868935);
//BiQuad LBHP2([3.913020539916823e-05,7.826041079833645e-05,3.913020539916823e-05, -1.982228929792529,0.982385450614126]);
BiQuad LBN1( 0.5, 0, 0.5, 0, 0);
BiQuad LBLP1(0000.087655548754006,   0000.175311097508013,   0000.087655548754006,    1.000000000000000,  -1.973344249781299,   0.973694871976315);
//BiQuad LBLP2( 0.000155148423475721, 0.000310296846951441, 0.000155148423475721,-1.96446058020523, 0.965081173899135);
BiQuadChain LeftBicepHP;
BiQuadChain LeftBicepN;
BiQuadChain LeftBicepLP;

//Filters Rechter Biceps
//In de volgorde High pass, notch, low pass
//BiQuad RBHP1(1, 2.000000023182220, 0.999999981993818, 1.893415601022358, 0.896255659548714);
//BiQuad RBHP2(1, 1.999999976817780, 0.999999976817780, 1.778313488139574, 0.790599715524785);
BiQuad RBHP1(0.995566972017647,  -1.991133944035294,   0.995566972017647, 1.000000000000000,  -1.991114292201654,   0.991153595868935);
//BiQuad RBHP2([3.913020539916823e-05,7.826041079833645e-05,3.913020539916823e-05, -1.982228929792529,0.982385450614126]);
BiQuad RBN1( 0.5, 0, 0.5, 0, 0);
BiQuad RBLP1(0000.087655548754006,   0000.175311097508013,   0000.087655548754006,  1.000000000000000,  -1.973344249781299,   0.973694871976315);
//BiQuad RBLP2( 0.000155148423475721, 0.000310296846951441, 0.000155148423475721,-1.96446058020523, 0.965081173899135);
BiQuadChain RightBicepHP;
BiQuadChain RightBicepN;
BiQuadChain RightBicepLP;


//Filters Rechter Quadriceps
//In de volgorde High pass, notch, low pass
//BiQuad RTHP1(1, 2.000000023182220, 0.999999981993818, 1.893415601022358, 0.896255659548714);
//BiQuad RTHP2(1, 1.999999976817780, 0.999999976817780, 1.778313488139574, 0.790599715524785);
BiQuad RTHP1(0.995566972017647,  -1.991133944035294 ,  0.995566972017647 ,1.000000000000000,  -1.991114292201654 ,  0.991153595868935);
//BiQuad RTHP2(1, 1.999999966458334, 0.999999966458334, -1.988418014198592, 0.988451549797368);
BiQuad RTN1( 0.5, 0, 0.5, 0, 0);
BiQuad RTLP1(0000.087655548754006  , 0.175311097508013 ,  0.087655548754006,  1.000000000000000 , -1.973344249781299  , 0.973694871976315);
//BiQuad RTLP2( 0.000155148423475721, 0.000310296846951441, 0.000155148423475721,-1.96446058020523, 0.965081173899135);
BiQuadChain RightLegHP;
BiQuadChain RightLegN;
BiQuadChain RightLegLP;
 
double emgLBHP;
double emgLBN;
double emgLBA;
//double emgLBLP;
//double emgAbsLBNotch;

double emgRBHP;
double emgRBN;
double emgRBA;
double emgRBLP;
double emgAbsRBNotch;

double emgRTN;
double emgRTHP;
double emgRTA;
//double emgRTLP;
//double emgAbsRTNotch;

double emgLBfiltered;
double emgRBfiltered;
double emgRTfiltered;
double emgLBraw;
double emgRBraw;
double emgRTraw;


double threshold_emgLB;
double threshold_emgRB;
double threshold_emgRT;
double threshold = 0.5;
double emgLB_max = 0.000;
double emgRB_max = 0.000;
double emgRT_max = 0.000;
double emgLB_maxrust = 0.000;
double emgRB_maxrust = 0.000;
double emgRT_maxrust = 0.000;
volatile int tijd = 0;
double timecalibration;

double emgsumLB;
double emgsumRB;
double emgsumRT;
double restmeanLB = 0.000;
double restmeanRB = 0.000;
double restmeanRT = 0.000;
//double emgmeansubLB;
//double emgmeansubRB;
//double emgmeansubRT;
double LBvalue;
double RBvalue;
double RTvalue;
double emgLBrust;
double emgRBrust;
double emgRTrust;
double RestmeanLB;
double RestmeanRB;
double RestmeanRT;

 
DigitalOut ledr(LED_RED);
DigitalOut ledg(LED_GREEN);
DigitalOut ledb(LED_BLUE);


AnalogIn emgLB(A0);     //read EMG left bicep
AnalogIn emgRB(A1);     //read EMG right bicep
AnalogIn emgRT(A2);     //read EMG right quadriceps

//HIDScope scope(6);      //aantal kanalen HIDScope (voor test 5, voor echt 6)
 
void rest()
// Rust. Hier wordt niets uitgevoerd. Wanneer button1
{
    // wordt ingedrukt gaan we naar de volgende state waar
    if (but1_pressed) {                                                         // de encoders worden gekalibreerd.
        state_changed = true;
        state = s_calibratie_encoder;
        but1_pressed = false;
    }
}
 
void calibratie_encoder()                                                       // calibratie encoder. Hier worden de encoders gekalibreerd en op
{
    // 0 gezet. Wanneer op button 1 wordt gedrukt gaan we naar de
    if (state_changed) {                                                        // demo modus, wanneer op button 2 wordt gedruk gaan we naar
        pc.printf("Started encoder calibration\r\n");                           // de EMG calibratie
        state_changed = false;
    }
    else if (but1_pressed) {
        pc.printf("Encoder has been calibrated \r\n");
        enc1_value = enc1.getPulses();
        enc2_value = enc2.getPulses();
        //enc1_value -= enc1_zero;
        //enc2_value -= enc2_zero;
        theta1 = (float)(enc1_value)/(float)(4200)*2*PI;                        // First arm rotated fully ccw
        theta2 = (float)(enc2_value)/(float)(4200)*2*PI;                        // second arm fully clockwise --> looks like a Z
        enc1_zero = enc1_value;
        enc2_zero = enc2_value;
        

        pc.printf("enc01: %i\r\n", enc1_zero);
        pc.printf("enc1value: %i\r\n", enc1_value);
        pc.printf("enc02: %i\r\n",enc2_zero);
        pc.printf("enc2value: %i\r\n", enc2_value);
        pc.printf("hoek 1: %f\r\n",theta1);
        pc.printf("hoek 2: %f\r\n",theta2);
        but1_pressed = false;
        state_changed = true;
        state = s_demonstratie;
    }
 
    else if (but2_pressed) {
        pc.printf("Encoder has been calibrated \r\n");
        enc1_value = enc1.getPulses();
        enc2_value = enc2.getPulses();
        //enc1_value -= enc1_zero;
        //enc2_value -= enc2_zero;
        theta1 = (float)(enc1_value)/(float)(4200)*2*PI;
        theta2 = (float)(enc2_value)/(float)(4200)*2*PI;
        enc1_zero = enc1_value;
        enc2_zero = enc2_value;
        

        pc.printf("enc01: %i\r\n", enc1_zero);
        pc.printf("enc1value: %i\r\n", enc1_value);
        pc.printf("enc02: %i\r\n",enc2_zero);
        pc.printf("enc2value: %i\r\n", enc2_value);
        pc.printf("hoek 1: %f\r\n",theta1);
        pc.printf("hoek 2: %f\r\n",theta2);
        
        but2_pressed = false;
        state_changed = true;
        state = s_EMGcalibratie;
        
    }
}
 
void filteren()
{
     //linkerbicep
    emgLBraw= emgLB.read(); //dit wordt: emgLBoffset
    emgLBHP=LeftBicepHP.step(emgLBraw);
    emgLBN=LeftBicepN.step(emgLBHP);
    //emgmeansubLB = emgLBN - RestmeanLB;
    emgLBA= fabs(emgLBN);
    emgLBfiltered=LeftBicepLP.step(emgLBA);
    //LBvalue = emgLBfiltered/emgLB_max;

   /* //to show if filter is working
    scope.set(0, emgLBraw);
    scope.set(1, emgLBfiltered);
    scope.set(2, emgRBraw);
    scope.set(3, emgRBfiltered);
    scope.set(4, emgRTraw);
    scope.set(5, emgRTfiltered); 
    scope.send();
*/
    //rechterbicep
    emgRBraw= emgRB.read();
    emgRBHP= RightBicepHP.step(emgRBraw);
    emgRBN=RightBicepN.step(emgRBHP);
    //emgmeansubRB = emgRBN - RestmeanRB;
    emgRBA= fabs(emgRBN);
    emgRBfiltered=RightBicepLP.step(emgRBA);
    //RBvalue = emgRBfiltered/emgRB_max;

    //Right Quadriceps
    emgRTraw= emgRT.read();
    emgRTHP= RightLegHP.step(emgRTraw);
    emgRTN= RightLegN.step(emgRTHP);
   // emgmeansubRT = emgRTHP - RestmeanRT;
    emgRTA= fabs(emgRTN);
    emgRTfiltered=RightLegLP.step(emgRTA);
    //RTvalue = emgRTfiltered/emgRT_max;
   
 
}
 
void EMGcalibration()
    {
        
            emgLBraw= emgLB.read(); //dit wordt: emgLBoffset
            emgLBHP=LeftBicepHP.step(emgLBraw);
            emgLBN=LeftBicepN.step(emgLBHP);
            emgLBA= fabs(emgLBN);
            emgLBfiltered=LeftBicepLP.step(emgLBA);
    
            emgRBraw= emgRB.read();
            emgRBHP= RightBicepHP.step(emgRBraw);
            emgRBN=RightBicepN.step(emgRBHP);
            emgRBA= fabs(emgRBN);
            emgRBfiltered=RightBicepLP.step(emgRBA);
        
    
            emgRTraw= emgRT.read();
            emgRTHP= RightLegHP.step(emgRTraw);
            emgRTN= RightLegN.step(emgRTHP);
            emgRTA= fabs(emgRTN);
            emgRTfiltered=RightLegLP.step(emgRTA);
    
        if (tijd > 1000 && tijd < 6000) {
            emgLBraw= emgLB.read();
            emgLBHP=LeftBicepHP.step(emgLBraw);
            emgLBN=LeftBicepN.step(emgLBHP);
            //emgmeansubLB = emgLBN - restmeanLB;
            emgLBA= fabs(emgLBN);
            emgLBfiltered=LeftBicepLP.step(emgLBA);
            if (emgLBfiltered > emgLB_max) {
                emgLB_max = emgLBfiltered;
            }
            //pc.baud(115200);
            //pc.printf("emgLB_max = %f \r\n", emgLB_max);// geen tekst printen in ticker want dat gaat mis xjes
            ledr=0; //led gaat aan zodra je linkerbicep moet aanspannen
        }
    
        else if (tijd > 7000 && tijd < 12000) {
            emgRBraw= emgRB.read();
            emgRBHP= RightBicepHP.step(emgRBraw);
            emgRBN=RightBicepN.step(emgRBHP);
            //emgmeansubRB = emgRBN - RestmeanRB;
            emgRBA= fabs(emgRBN);
            emgRBfiltered=RightBicepLP.step(emgRBA);
            if (emgRBfiltered > emgRB_max) {
                emgRB_max = emgRBfiltered;
            }
            //pc.printf("emgRB_max = %f \r\n", emgRB_max);
            ledr=0; //led gaat uit zodra je rechterbicep moet aanspannen
        }
    
    
        else if (tijd > 13000 && tijd < 18000) {
            emgRTraw= emgRT.read();
            emgRTHP= RightLegHP.step(emgRTraw);
            emgRTN= RightLegN.step(emgRTHP);
            //emgmeansubRT = emgRTHP - RestmeanRT;
            emgRTA= fabs(emgRTN);
            emgRTfiltered=RightLegLP.step(emgRTA);
            if (emgRTfiltered > emgRT_max) {
                emgRT_max = emgRTfiltered;
            }
            //pc.printf("emgRT_max = %f \r\n", emgRT_max);
            ledr=0; //led gaat aan zodra je rechterbeenspier moet aanspannen
        } else if (tijd > 18000) {
            ledr=1; //led gaat uit zodra kalibratie voltooid
            state=s_EMG;
            state_changed=true;
            
            //pc.printf("Calibration finished!");
        } else {
            ledr=1;
            }
     
        pc.printf("%i", tijd);
        tijd++;
        threshold_emgLB = threshold*emgLB_max;   //bepaal threshold, nu op 0.15*maximale waarde. 
        threshold_emgRB = threshold*emgRB_max; 
        threshold_emgRT = threshold*emgRT_max;
    
    
    // if threshold_emgx is reached, brushingmodes activated
}
 
 
 
void SetMotor1(float motorValue1)
{
    // gegeven motorValue1 <=1, schrijft snelheid naar pwm.
    // MotorValues buiten range worden afgekapt dat ze binnen de range vallen.
    motor1_pwm.write(fabs(motorValue1) > 1 ? 1 : fabs(motorValue1));
}
 
void SetMotor2(float motorValue2)
{
    motor2_pwm.write(fabs(motorValue2) > 1 ? 1 : fabs(motorValue2));
}
 
void brushingmode()
{
    
        //boven draait arm 1 aan, dus motor 1 
        //onder draait arm 2 aan, dus motor 2 
        if (emgLBfiltered > threshold_emgLB){      //tandenborstel naar links (cw/ccw) //direction1/2 = cw/ccw
        
        motor2_direction.write(direction1);   //motor 2 gaat cw
        //motor2_direction.write(direction1 = !direction1); //is counterclockwise
        motor2_pwm.write(maxPWM2);
        ledr=0;
        
        
        }else if (emgRBfiltered > threshold_emgRB){
            motor2_pwm.write(maxPWM2);
            motor2_direction.write(direction1 = !direction1); //is counterclockwise
        //tandenborstel naar rechts
        ledg=0;
        
        
        }else if (emgRTfiltered > threshold_emgRT){
        motor1_direction.write(direction1 = !direction1);
        motor1_pwm.write(maxPWM1);
        //tandenborstel naar achter
        ledb=0;
        
        }else if (emgRBfiltered > threshold_emgRB && emgLBfiltered > threshold_emgLB){
        motor1_direction.write(direction1); //motor 1 gaat cw
        motor1_pwm.write(maxPWM1);
        //tandenborstel naar voren
        ledg=0;
        ledr=0;
}
}
 
void demonstratie()
{
    if (state_changed) {                                                        //
        pc.printf("Demonstratie gestart\r\n")   ;                                //
        state_changed = false;
    }
}
 
 
void RKI()
{
    Lq1 = q1*r_trans;
    Cq2 = q2/gear2;                               //1.25 is gear ratio voor q2
 
    q1_dot = (v_x + (v_y*(L2*sin(q2/gear2)))/(L0 + q1*r_trans + L2*cos(q2/gear2)))/r_trans;
    q2_dot = (gear2*v_y)/(L0 + q1*r_trans + L2*cos(q2/gear2));
 
    q1_ii = q1 + q1_dot*dt;
    q2_ii = q2 + q2_dot*dt;
 
    q1 = q1_ii;
    q2 = q2_ii;
 
}
 
double GetReferencePosition()
{
    double Potmeterwaarde = potMeter2.read();
 
    double refP = Potmeterwaarde*maxwaarde;
    return refP;
}
 
double GetReferencePosition2()
{
    double potmeterwaarde2 = potMeter1.read();
    double refP2 = potmeterwaarde2*maxwaarde;
    return refP2;
}
 
double FeedBackControl(double error, double &e_int)
{
    //double Proportional= kp*error1;
    double motorValue1= kp*error1;
    //e_int = e_int+dt*error1;
    //double Integrator = ki*e_int;
 
   // motorValue1 = Proportional + Integrator ;
    return motorValue1;
}
 
double FeedBackControl2(double error2, double &e_int2)
{
    //double Proportional2= kp2*error2;
    double motorValue2= kp2*error2;
    //e_int2 = e_int2+dt*error2;
    //double Integrator2 = ki2*e_int2;
 
    //double motorValue2 = Proportional2 + Integrator2 ;
    return motorValue2;
}
 
void MeasureAndControl()
{
    // RKI aanroepen
    //RKI();
    // hier the control of the 1st control system
    refP = GetReferencePosition();                    //moet eigenlijk nog met RKI
    Huidigepositie1 = enc1.getPulses()*4200;
    //error1 = (refP - Huidigepositie1);// make an error
    error1 = refP ;// ffwd
    motorValue1 = FeedBackControl(error1, e_int);
    SetMotor1(motorValue1);
    // hier the control of the 2nd control system
    refP2 = GetReferencePosition2();
    Huidigepositie2 = enc2.getPulses();
    //error2 = (refP2 - Huidigepositie2);// make an error
    error2 = refP2;// ffwd
    motorValue2 = FeedBackControl2(error2, e_int2);
    SetMotor2(motorValue2);
    //pc.printf("refP = %d, huidigepos = %d, motorvalue = %d, refP2 = %d, huidigepos2 = %d, motorvalue2 = %d \r\n", refP, Huidigepositie1, motorValue1, refP2, Huidigepositie2, Huidigepositie2);
 
}
 
void direction()
{
    if (butsw2==0) {
        if (A==true) { // zodat het knopje 1 x wordt afgelezen
            // richting veranderen
            motor1_direction.write(direction1 = !direction1);
            pc.printf("direction: %s\r\n\n", direction1 ? "clockwise" : "counter clockwise");
            A=false;
        }
    } else {
        A=true;
    }
 
    if (butsw3==0) {
        if (B==true) {
            motor2_direction.write(direction2 = !direction2);
            pc.printf("direction: %s\r\n\n", direction2 ? "clockwise" : "counter clockwise");
            B=false;
        }
    } else {
        B=true;
    }
 
}
 
void but1_interrupt()
{
    /*if(but2.read()) {//both buttons are pressed
     failure_occurred = true;
    }*/
    but1_pressed = true;
    pc.printf("Button 1 interrupt active \n\r");
}
 
void but2_interrupt()
{
    /*if(but1.read()) {//both buttons are pressed
      failure_occurred = true;
    }*/
    but2_pressed = true;
    pc.printf("Button 2 interrupt active \n\r");
}

 void state_machine()
{
    //run current state
    switch (state) {
        case s_idle:                                                            // in deze state gebeurd niets. Als op knop 1 wordt gedrukt
            rest();                                                             // gaan we over naar s_calibratie_encoder
            break;
        case s_calibratie_encoder:                                              // in deze state worden de encoders gekalibreerd. knop 1 -> s_demonstratie
            calibratie_encoder();                                               // knop 2 -> s_EMGcalibratie
            break;
        case s_demonstratie:                                                    // in deze state kunnen de motors worden bestuurd met de potmeters
            demonstratie();
            MeasureAndControl();                                                // en switch 2 en 3( pot1,sw2->motor1 / pot2,sw3->motor2
            direction();                                                        // als op knop 2 wordt gedrukt komen we in de s_idle state
            if (but2_pressed) {
                pc.printf("fail. \r\n");
                but2_pressed = false;
                state_changed = true;
                state = s_idle;
            }
            break;
            
        case s_EMGcalibratie:
            EMGcalibration();
            break;
            
        case s_EMG:
            ledg=1;
            ledr=1;
            ledb=1;
            state_changed=false;
            filteren();
            brushingmode();
               if (but2_pressed) {
                pc.printf("fail. \r\n");
                but2_pressed = false;
                state_changed = true;
                state = s_idle;
                }
            break;
 
        }
}

int main()
{
    pc.baud(115200);
    pc.printf("Executing main()... \r\n");
    state = s_idle;
    ledg=1;
    ledr=1;
    ledb=1;
 
    LeftBicepHP.add( &LBHP1 );  //BiQuadChain bqc;   //bqc.add( &bq1 ).add( &bq2 );
    LeftBicepN.add( &LBN1 );
    LeftBicepLP.add( &LBLP1 );


    RightBicepHP.add( &RBHP1 );
    RightBicepN.add( &RBN1 );
    RightBicepLP.add( &RBLP1 );

    RightLegHP.add( &RTHP1 );
    RightLegN.add( &RTN1 );
    RightLegLP.add( &RTLP1 );
 
    but1.fall(&but1_interrupt);
    but2.fall(&but2_interrupt);
    loop_ticker.attach(&state_machine, dt); 
    pc.printf("state_machine active!\n\r");
 
}