File content as of revision 30:8191e8541a0a:

#include "mbed.h"
#include "MODSERIAL.h"
#include "BiQuad.h"
#include "HIDScope.h"
#include <math.h>
#include "QEI.h"

//ATTENTION:    set mBed to version 151
//              set QEI to version 0, (gebruiken wij (nog) niet, is voor encoder)
//              set MODSERIAL to version 44
//              set HIDScope to version 7
//              set biquadFilter to version 7

AnalogIn potmeter1          (A0);                   //First raw EMG signal input
AnalogIn potmeter2          (A1);                   //Second raw EMG signal input

InterruptIn encoderA1       (D9);
InterruptIn encoderB1       (D8);
InterruptIn encoderA2       (D12);
InterruptIn encoderB2       (D13);

InterruptIn button1         (D10);                  
InterruptIn button2         (D11);

DigitalOut directionpin1    (D4);
DigitalOut directionpin2    (D7);

PwmOut pwmpin1              (D5);
PwmOut pwmpin2              (D6);

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


MODSERIAL pc(USBTX, USBRX);                       //Serial communication to see if the code works step by step, turn on if hidscope is off
QEI encoder1 (D9, D8, NC, 8400 , QEI::X4_ENCODING);
QEI encoder2 (D12, D13, NC, 8400 , QEI::X4_ENCODING);

//HIDScope    scope( 6 );                             //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered

//Tickers
Ticker      func_tick;  
                     

//Global variables
const float T   = 0.02f;                           //Ticker period             Deze wordt ook gebruikt in de PID, moet die niet anders???
const float T2  = 0.02f;

// Inverse Kinematica variables
const double L1 = 0.208;                                  // Hoogte van tafel tot joint 1
//const double L2 = 0.288;                                  // Hoogte van tafel tot joint 2
const double L3 = 0.212;                                  // Lengte van de arm
const double L4 = 0.089;                                  // Afstand van achterkant base tot joint 1
//const double L5 = 0.030;                                  // Afstand van joint 1 naar joint 2
const double r_trans = 0.035;                             // Kan gebruikt worden om om te rekenen van translation naar shaft rotation 

// Variërende variabelen inverse kinematics: 
double q1ref = 0.0;                                   // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is
double q2ref = 0.0;                                   // Huidige motorhoek van joint 2 zoals bepaald uit referentiesignaal --> checken of het goede type is
double v_x;                                         // Desired velocity end effector in x direction --> Determined by EMG signals
double v_y;                                         // Desired velocity end effector in y direction --> Determined by EMG signals

double Lq1;                                         // Translatieafstand als gevolg van motor rotation joint 1
double Cq2;                                         // Joint angle of the system (corrected for gear ratio 1:5)

double q1_dot=0.0;                                      // Benodigde hoeksnelheid van motor 1 om v_des te bereiken
double q2_dot=0.0;                                      // Benodigde hoeksnelheid van motor 2 om v_des te bereiken 

double q1_ii=0.0;                                       // Reference signal for motorangle q1ref 
double q2_ii=0.0;                                       // Reference signal for motorangle q2ref

//Variables PID controller
double PI = 3.14159;
double Kp1 = 17.5;                                  //Motor 1           eerst 17.5 , nu 1
double Ki1 = 1.02;
double Kd1 = 23.2;
double encoder_radians1=0;
double err_integral1 = 0;
double err_prev1 = 0;


double Kp2 = 17.5;                                  //Motor 2            eerst 17.5, nu 1
double Ki2 = 1.02;
double Kd2 = 23.2;
double encoder_radians2=0;
double err_integral2 = 0;
double err_prev2 = 0;

int start_control = 0;
double emg_cal = 1;


//--------------Functions----------------------------------------------------------------------------------------------------------------------------//


//------------------ Filter EMG + Calibration EMG --------------------------------//


//---------PID controller motor 1 + motor control 1 & 2-----------------------------------------------------------//
double PID_control(double err, const double Kp, const double Ki, const double Kd, double &err_integral, double &err_prev)
{
    pc.printf("ik doe het, PDI \n\r");

    err_integral = 0;
    err_prev = err; // 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 * err;

    //Integral part  
      err_integral = err_integral + err * T;
      double u_i = Ki * err_integral;
    
    // Derivative part
      double err_derivative = (err - err_prev)/T;
      double filtered_err_derivative = LowPassFilter.step(err_derivative);
      double u_d = Kd * filtered_err_derivative;
      err_prev = err;
      
    
      // Sum all parts and return it
      return u_k + u_i + u_d;  
    }

void engine_control1()                                           //Engine 1 is translational engine, connected with left side pins
{
        //pc.printf("ik doe het, engine control 1\n\r");
        encoder_radians1 = encoder1.getPulses()*(2*PI)/8400.0;
        pc.printf("encoder1 %d \n\r",encoder1.getPulses());
        pc.printf("encoder_radians1 %f \n\r",encoder_radians1);
        double err1 = q1ref - encoder_radians1;
        pc.printf("err1 = %f\n\r",err1);
        double u1 = PID_control(err1, Kp1, Ki1, Kd1, err_integral1, err_prev1);                               //PID controller function call
        
        pc.printf("u1 = %f\n\r",u1);
        
        if(encoder1.getPulses()<12000 && encoder1.getPulses()>-1)                              //limits translation in counts, eerst 12600
            {
                 pwmpin1 = fabs(u1);                                         //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
                 directionpin1.write(u1<0);
            }
        else
            {
                pwmpin1 = 0;
            }  
}

void engine_control2()                                             //Engine 2 is rotational engine, connected with right side wires
{
        encoder_radians2 = encoder2.getPulses()*(2*PI)/8400.0;
        pc.printf("encoder2 %f \n\r",encoder2.getPulses());
        pc.printf("encoder_radians2 %d \n\r",encoder_radians2);
        double err2 = q2ref - encoder_radians2;
        pc.printf("err2 = %f\n\r",err2);
        double u2 = PID_control(err2, Kp2, Ki2, Kd2, err_integral2, err_prev2);                             //PID controller function call
        pc.printf("u2 = %f\n\r",u2);

        if(encoder2.getPulses()<5250 && encoder2.getPulses()>-5250)                              //limits rotation, in counts                
         {
            pwmpin2 = fabs(u2);                                       //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
            directionpin2.write(u2>0);
         }
        else
         {
            pwmpin2 = 0;
         }
}


//------------------ Inversed Kinematics --------------------------------//


void inverse_kinematics(double v_x, double v_y)
{
    
    pc.printf("v_x is %f en v_y is %f\n\r",v_x, v_y);
    
    Lq1 = q1ref*r_trans;                            
    Cq2 = q2ref/5.0;                               

    q1_dot = v_x + (v_y*(L1 + L3*sin(Cq2)))/(L4 + Lq1 + L3*cos(Cq2));     
    q2_dot = v_y/(L4 + Lq1 + L3*cos(Cq2));                                       

    q1_ii = q1ref + (q1_dot/r_trans)*T;                       
    q2_ii = q2ref + (q2_dot*5.0)*T; 
        
    q1ref = q1_ii;
    q2ref = q2_ii; 
    
    
    pc.printf("q1ref is %f en q2ref is %f\n\r",q1ref, q2ref);

    
    //start_control = 1;
    engine_control1();                           
    engine_control2();
}

void v_des_calculate_qref()
{
    double m1 = (potmeter1*2.0)-1.0;
    double m2 = (potmeter2*2.0)-1.0;
    
                if(m1>0.5)                   //If the filtered EMG signal of muscle 1 is higher than the threshold, motor 1 will turn
                {
                    v_x = 0.5;                          //beweging in +x direction
                    v_y = 0.0;
                    ledr = 0;                           //red
                    ledb = 1;
                    ledg = 1;
                    
                }
                else if(m2>0.5)              //If the filtered EMG signal of muscle 2 is higher than the threshold, motor 1 and 2 will turn
                {
                    v_y = 0.5;                          //beweging in +y direction
                    v_x = 0.0;
                    ledr = 1;                           //green
                    ledb = 1;
                    ledg = 0;
                }
               
                else if(m1 < -0.5)              //If the filtered EMG signal of muscle 0 is higher than the threshold, motor1 will turn in 1 direction
                {
                    v_x = -0.5;
                    v_y = 0;
                    ledr = 1;                           //Blue
                    ledb = 0;
                    ledg = 1;  
                }             
                
                else if(m2 < -0.5)              //If the filtered EMG signal of muscle 0 is higher than the threshold, motor1 will turn in 1 direction
                {
                    v_x = 0;
                    v_y = -0.5;
                    ledr = 1;                           //Blue
                    ledb = 0;
                    ledg = 1;  
                }  
                
                
                else                                    //If not higher than the threshold, motors will not turn at all
                {                    
                    v_x = 0;
                    v_y = 0;
                    ledr = 0;                           //white
                    ledb = 0;
                    ledg = 0;
                    //pwmpin1 = 0;
                    //pwmpin2 = 0;
                }
      
        inverse_kinematics(v_x, v_y);                           //Call inverse kinematics function
       
}

//------------------ Start main function --------------------------//


int main()
{        
        pc.baud(115200);
        pc.printf("Hello World!\r\n");                            //Serial communication only works if hidscope is turned off.
        pwmpin1.period_us(60);                                    //60 microseconds PWM period, 16.7 kHz 
        
        func_tick.attach(&v_des_calculate_qref,T2);                 //v_des determined every T

        
    while(true)
    {
        //engine_control1_tick.attach(&engine_control1,T2);
        //engine_control2_tick.attach(&engine_control2,T2);
        
           // HIDScope_tick.attach(&HIDScope_sample,T);             //EMG signals raw + filtered to HIDScope every T sec.
           
        pc.printf("Encoder engine 1 %d\n\r",encoder2.getPulses());
        pc.printf("Encoder engine 2 %d\n\r",encoder1.getPulses());
        wait(0.1f);
    }      
}