File content as of revision 5:224c1fe73a8f:

#include "QEI.h"
#include "math.h"
#include "mbed.h"
//#include "HIDScope.h" //set mbed library version to 119 for HIDScope to work
#include "MODSERIAL.h"
#include "BiQuad.h"

//left pot to set reference position.
//right pot to set Kp, right pot sets Ki when (right)button is pressed down


//--------------object creation------------------ 
Serial pc(USBTX, USBRX);
//Use X4 encoding.
//QEI wheel(p29, p30, NC, 624, QEI::X4_ENCODING);
//Use X2 encoding by default.
QEI enc1(D13, D12, NC, 32); //enable the encoder
QEI enc2(D15, D14, NC, 32); //enable the encoder
PwmOut M1_speed(D6);
PwmOut M2_speed(D5);
DigitalOut M1_direction(D7);
DigitalOut M2_direction(D4);
AnalogIn potmeter(A0);      //left pot
AnalogIn potmeter2(A1);     //right pot
InterruptIn button1(D2);       //hardware interrupt for stopping motors - right button
DigitalIn LimSW1(D9);
DigitalIn LimSW2(D8);
DigitalIn HomStart(D3); // - left button

BiQuad bqlowpass(0.4354, 0.8709, 0.4354, 0.5179, 0.2238);
//create scope objects - note: script won't run when HID usb port is not connected
//HIDScope scope(5); //set # of channels

Ticker motor_update1;
Ticker motor_update2;
Ticker error_update;

//Define objects EMG filter
AnalogIn    emg2( A2 );
AnalogIn    emg3( A3 );
AnalogIn    emg4( A4 );
AnalogIn    emg5( A5 );


Ticker      sample_timer;
Ticker      scope_timer;

DigitalOut  ledred(LED_RED);
DigitalOut  ledblue(LED_BLUE);
DigitalOut  ledgreen(LED_GREEN);
DigitalOut  led2(LED_RED);
DigitalIn   button_cal (SW2);


//-----------------variable declaration----------------
int pwm_freq = 500;
float set_speed;
float reference_pos;



double M1_home; 
double M1_error_pos = 0;
float M1_Kp = 2;
float M1_Ki = 4;
float M1_Kd = 0.19;
double M1_e_int=0;
double M1_e_prior=0;

double M2_home; 
double M2_error_pos = 0;
float M2_Kp = 1.5;
float M2_Ki = 0.5;
float M2_Kd = 0;
double M2_e_int=0;
double M2_e_prior=0;

double setpoint = 0;

double M1_rel_pos;
double M2_rel_pos;

float Ts = 0.002; //500hz sample freq

float gain_factor = 1.5;
double x_vel;
double y_vel;

bool M1homflag;
bool M2homflag;
bool Homstartflag;
  
/** Sample function
 * this function samples the emg and sends it to HIDScope
 **/
 int P= 200;
 double A[200];
 
 double sig_2_abs;
 double sig_3_abs;
 double sig_4_abs;
 double sig_5_abs;
 double movmean2;
 double movmean3;
 double movmean4;
 double movmean5;
 double emgsample2;
 double emgsample3;
 double emgsample4;
 double emgsample5;
 
enum        States {cal2,cal3,cal4,cal5};
int         state;

double      movmean_cal[5000];
double      movmean_max2=0;
double      movmean_max3=0;
double      movmean_max4=0;
double      movmean_max5=0; 


// Filters 
BiQuadChain bqc;
BiQuad bq1( 0.6844323315947305,1.368864663189461, 0.6844323315947305,1.2243497755555954,0.5133795508233265); //lp?
BiQuad bq2( 0.6844323315947306, -1.3688646631894612,  0.6844323315947306,   -1.2243497755555959, 0.5133795508233266); //hp?
BiQuad bq3(  0.7566897754116633, -1.2243497755555959,  0.7566897754116633,   -1.2243497755555959, 0.5133795508233266); // notch
    
    
//Function emgfilter signal 2
    double emgfilter2(double emgsample2)
        {
        //filter and take abs of input
        double sig_2_filt = bqc.step(emgsample2);
        sig_2_abs = abs(sig_2_filt);
        //calculate moving mean of the filtered signal
        for(int i = P-1; i >= 0; i--){
            if (i == 0) {
            A[i] = sig_2_abs;
            }
            else {
             A[i] = A[i-1];
             }   
                                    }
            double sum2 = 0;
            // deze for-loop sommeert de array
            for (int n = 0; n < P-1; n++) 
                {
                sum2 = sum2 + A[n];
                movmean2 = sum2/P; //dit is de moving average waarde  
                }
                double set_speed2=movmean2/movmean_max2;
                return(set_speed2);
                }
            
    
//Signal 3
    double emgfilter3(double emgsample3){
        //filter and take abs of input
        double sig_3_filt = bqc.step(emgsample3);
        sig_3_abs = abs(sig_3_filt);
    
        //calculate moving mean of the filtered signal
        for(int i = P-1; i >= 0; i--){
        if (i == 0) {
            A[i] = sig_3_abs;
            }
         else {
             A[i] = A[i-1];
             }   
            }
    double sum3 = 0;
    // deze for-loop sommeert de array
    for (int n = 0; n < P-1; n++) {
    sum3 = sum3 + A[n];
    movmean3 = sum3/P; //dit is de moving average waarde    
        }
    double set_speed3=movmean3/movmean_max3;
                return(set_speed3);
    }
//Signal 4
    double emgfilter4(double emgsample4)
    {
        //filter and take abs of input
        double sig_4_filt = bqc.step(emgsample4);
        sig_4_abs = abs(sig_4_filt);
    
        //calculate moving mean of the filtered signal
        for(int i = P-1; i >= 0; i--){
             if (i == 0) {
                A[i] = sig_4_abs;
                }
            else {
                A[i] = A[i-1];
                }   
                                    }
    double sum4 = 0;
    // deze for-loop sommeert de array
    for (int n = 0; n < P-1; n++) {
    sum4 = sum4 + A[n];
    movmean4 = sum4/P; //dit is de moving average waarde    
        }
    double set_speed4=movmean4/movmean_max4;
    return(set_speed4);
    }
    //Signal 5  
    double emgfilter5(double emgsample5)
    {
        //filter and take abs of input
        double sig_5_filt = bqc.step(emgsample5);
        sig_5_abs = abs(sig_5_filt);
    
        //calculate moving mean of the filtered signal
        for(int i = P-1; i >= 0; i--){
            if (i == 0) {
                A[i] = sig_5_abs;
                }
            else {
                A[i] = A[i-1];
             }   
                }
    double sum5 = 0;
    // deze for-loop sommeert de array
    for (int n = 0; n < P-1; n++) {
    sum5 = sum5 + A[n];
    movmean5 = sum5/P; //dit is de moving average waarde    
    }
    double set_speed5=movmean5/movmean_max5;
    return(set_speed5);
}

void sample(){
    emgsample2 = emg2.read();
    emgsample3 = emg3.read();
    emgsample4 = emg4.read();
    emgsample5 = emg5.read();
    double x_vel = emgfilter2(emgsample2)-emgfilter3(emgsample3); 
    double y_vel  = emgfilter4(emgsample4)-emgfilter5(emgsample5);
    }
    
//Calibration
void calibration_switch()
{
    switch(state) {
        case cal2:
            ledred=0;
            for (int i=0; i<=3500; i++){
            if (movmean2>movmean_max2) {
                movmean_max2=movmean2;
                }}
                state=cal3;
        break;
                
        case cal3:        
            ledred=1;
            ledblue=0;
            for (int i=0; i<=3500; i++){
            if (movmean3>movmean_max3) {
                movmean_max3=movmean3;
                }}
                state=cal4;
        break;
                
        case cal4:        
            ledblue=1;
            ledgreen=0;
            for (int i=0; i<=3500; i++){
            if (movmean4>movmean_max4) {
                movmean_max4=movmean4;
                }}
                state=cal5;
        break;
                
        case cal5:
            ledgreen=1;
            ledred=0;
            ledblue=0;
            for (int i=0; i<=3500; i++){
            if (movmean5>movmean_max5) {
                movmean_max5=movmean5;
                }}
        break;
} //end switch  
} //end calibration_switch

void calibration () {
         while(button1==0) {
                state=cal2;
                calibration_switch();
                    }
                    }

 
void kinemtaica(double q1, double q2, double vdx, double vdy, double &q1_new, double &q2_new){
  vdx=x_vel*gain_factor;
  vdy=y_vel*gain_factor;
  double q1_dot = (2.5*cos(q1+q2))/(sin(q2)) *vdx + (2.5*sin(q1+q2))/(sin(q2))*vdy;
  double q2_dot = -(0.3*cos(q1+q2)+0.4*cos(q1))/(0.12*sin(q2))*vdx -(0.3*sin(q1+q2)+0.4*sin(q2))/(0.12*sin(q2))*vdy;
  q1_new = q1 +q1_dot*Ts;
  q2_new = q2 +q2_dot*Ts;
 return;
}

float PID(double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prior){ //PID calculator
    e_int += Ts*e;
    double e_diff = (e-e_prior)/Ts;
    e_prior = e;
    double e_diff_filter = bqlowpass.step(e_diff);
    return(Kp*e+Ki*e_int+Kd*e_diff_filter);
    }

void M1_control(){
    
    //call PID func and set new motor speed
    set_speed = PID(M1_error_pos,M1_Kp,M1_Ki,M1_Kd,Ts,M1_e_int,M1_e_prior);
    if(set_speed > 0){
     M1_speed.write(abs(set_speed));
     M1_direction.write(0);
     }
    else if (set_speed < 0){
     M1_speed.write(abs(set_speed));
     M1_direction.write(1);
     }
    else{M1_speed.write(0);}
    
 }
 
 void M2_control(){
    set_speed = PID(M2_error_pos,M2_Kp,M2_Ki,M2_Kd,Ts,M2_e_int,M2_e_prior);
    if(set_speed > 0){
     M2_speed.write(abs(set_speed));
     M2_direction.write(0);
     }
    else if (set_speed < 0){
     M2_speed.write(abs(set_speed));
     M2_direction.write(1);
     }
    else{M2_speed.write(0);}       
    pc.printf("M2 integral = %f\n\r", M2_e_int);
  }
 
 void homing_system () {
    LimSW1.mode(PullDown);
    LimSW2.mode(PullDown);
    M1_speed.write(0);
    M2_speed.write(0);
    M1_direction.write(0);
    M2_direction.write(1);
    
       while(1){
            if (HomStart.read() == 0){
                  setpoint += 0.003; //move setpoint 0.2 radian per second (at 100hz)
                  pc.printf("Homing... \n\r");
            }     
            
            double M1_rel_pos = enc1.getPulses()/32.0/131.25*2.0*3.1416; 
            double M2_rel_pos = enc2.getPulses()/32.0/131.25*2.0*3.1416;  
            if(LimSW1.read() == 0){
                  M1_error_pos = 1.5*setpoint - M1_rel_pos;
            }
            if(LimSW2.read() == 0){
                M2_error_pos = -(0.5*setpoint - M2_rel_pos);
            }
            M1_control();
            M2_control();
            
        
        if(LimSW1.read() ==  1){
          M1_error_pos = 0;
          M1_speed.write(0);
          M1_home = enc1.getPulses()/32.0/131.25*2.0*3.1416; //in radians
        }    
        if (LimSW2.read() == 1) {
          M2_error_pos = 0;
          M2_speed.write(0);
          M2_home = enc2.getPulses()/32.0/131.25*2.0*3.1416; //in radians
        }
            
        if (LimSW1.read() == 1 && LimSW2.read() ==1) {
                pc.printf("Homing finished \n\r");
                M1_speed.write(0);
                M2_speed.write(0);
                wait(0.5);
                M1_home = enc1.getPulses()/32.0/131.25*2.0*3.1416; //in radians
                M2_home = enc2.getPulses()/32.0/131.25*2.0*3.1416; //in radians
                //break;
                while(1); //stop after homing.
        }
        wait(0.01);
       }

}
 
 void scopesend(){
  
     
     
     }
 void StopMotors(){
     while(1){
         M1_speed.write(0);
         M2_speed.write(0);
         }
    }
 
void geterror(){ 
    double M1_rel_pos = enc1.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians
    double M2_rel_pos = enc2.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians

    float Arm1_home = 122.0/180.0*3.1416;//home position of large link attached to base in radians - 112 degrees
    float Arm2_home = 19.0/180.0*3.1416;//home position of small link attached to base
    
    double M1_actual_pos = Arm1_home + (M1_rel_pos - M1_home)/2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!! - Also known as angle theta
    double M2_actual_pos = Arm2_home + (M2_rel_pos - M2_home)/2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!! - Also known as angle Alpha
    
    double q1 = M1_actual_pos;
    double q2 = q1 + M2_actual_pos; //see drawing
    
    //double M1_reference_pos = 1+potmeter.read()*0.5*3.1416; //should cover the right range - radians
    double M1_reference_pos = 0.5*3.1416-potmeter.read(); //should cover the right range - radians
    double M2_reference_pos = 0.5+potmeter2.read()*0.25*3.1416;
        
    
    //M2_error_pos = M2_reference_pos - M2_actual_pos;
    M2_error_pos = 0;
    if(M1_reference_pos > Arm1_home){
        M1_reference_pos = Arm1_home;
    }
    else{
        M1_error_pos = M1_reference_pos - M1_actual_pos;
    }
    if(M2_reference_pos > Arm2_home){
        M2_reference_pos = Arm2_home;
    }
    else{
        M2_error_pos = M2_reference_pos - M2_actual_pos;
    }
}
 
int main() {
    calibration();
    //add biquad sections to the chain
    bqc.add( &bq1 ).add( &bq2 ).add( &bq3 );
    
    sample_timer.attach(&sample, 0.002);
    //scope_timer.attach(&scopesend, 0.002);
                
    /*empty loop, sample() is executed periodically*/
    while(1) {}
    //initialize serial comm and set motor PWM freq
M1_speed.period(1.0/pwm_freq);
M2_speed.period(1.0/pwm_freq);
pc.baud(115200);
pc.printf("starting homing function now. Push button to start procedure \n\r");
//commence homing procedure
homing_system();
pc.printf("Setting home position complete\n\r");
//attach all interrupt
pc.printf("attaching interrupt tickers now \n\r");
button1.fall(StopMotors);     //stop motor interrupt
motor_update1.attach(&M1_control,0.01);
motor_update2.attach(&M2_control,0.01);
error_update.attach(&geterror,0.01);

pc.printf("initialization complete - position control of motors now active\n\r");
    
    while(1){   
        
     
    }
 
}