File content as of revision 5:867fe891b990:

#include "mbed.h"
#include "MODSERIAL.h"
#include "encoder.h"
#include "HIDScope.h"
#include "math.h"


///// points..... mooi maken calib
Serial pc(USBTX,USBRX);
HIDScope scope(4);          // definieerd het aantal kanalen van de scope

// Define Tickers and control frequencies
Ticker          controller1, controller2, main_filter, cartesian;        // definieer de ticker die controler1 doet
    // Go flag variables
    volatile bool motor1_go = false, motor2_go = false, emg_go = false, cart_go = false;

    // Frequency control
    double controlfreq = 50 ;    // controlloops frequentie (Hz)
    double controlstep = 1/controlfreq; // timestep derived from controlfreq


//MOTOR OUTPUTPINS
// motor 1
    PwmOut motor1_aan(D6);      // PWM signaal motor 2 (uit sheets)
    DigitalOut motor1_rich(D7); // digitaal signaal voor richting
// motor 2
    PwmOut motor2_aan(D5);
    DigitalOut motor2_rich(D4);

// ENCODER INPUTPINS
Encoder motor1_enc(D12,D11);        // encoder outputpins
Encoder motor2_enc(D10,D9);

////////
// physical constants
const double L = 36;           // lenght of arms
const double pi = 3.1415926535897;
///////////////////////////
// Main control loop transfer variables
// here all variables that transfer bewtween the primary control functions

// filter to calibration
double output1;
double output2;

// filter to x-y
double output1_amp;
double output2_amp;

// x-y to motor control
double phi_one;
double phi_two;


// EXTRA INPUTS AND REQUIRED VARIABLES
//POTMETERS
    AnalogIn potright(A4);      // define the potmeter outputpins
    AnalogIn potleft(A5);
    
// Analoge input signalen defineren
AnalogIn    EMG_in(A0);                 // EMG_in.read kan je nu gebruiken om het analoge signaal A2 uit te lezen
AnalogIn    EMG_int(A2);                // deze leest A3 uit

// BUTTONS 
    // control flow             
    DigitalIn   buttonlinks(PTA4);       
    DigitalIn   buttonrechts(PTC6);
        // init values
        bool loop_start = false;
        bool calib_start = false;
        
    // direction control
    DigitalIn   reverse_button_links(D0);
    DigitalIn   reverse_button_rechts(D1); 
        // init values
        bool reverse_links = false;
        bool reverse_rechts = false;
        
        
// LED
    DigitalOut ledred(LED1);
    DigitalOut ledgreen(LED2);
    DigitalOut ledblue(LED3);

// REFERENCE SIGNAL SETTINGS
    double input_threshold = 0.25;   // the minimum value the signal must have to change the reference.
 // Define storage variables for reference values
    double c_reference1 = 0;
    double c_reference2 = 0;
    
// define start up variables
    double start_up_time = 2;
    double start_loops = start_up_time*controlfreq;
    int rc1 = 0;
    int rc2 = 0;
    
// limit  angles (in radians)
    // motor1
    const double limlow1 = 1;             // min height in rad
    const double limhigh1 = 6.3;            // max height in rad
    // motor 2
    const double limlow2 = -4.0;            // maximum height, motor has been inverted due to transmission
    const double limhigh2 = 2.5;              // minimum height in rad
    
// Define the maximum rate of change for the reference (velocity)
    double Vmax = 3;            // rad/sec

// CONTROLLER SETTINGS
    // motor 1
    const double m1_Kp = 5;         // Proportional constant     
    const double m1_Ki = 0.5;         // integration constant
    const double m1_Kd = 0.4;         // differentiation constant
    // motor 2
    const double m2_Kp = 2;
    const double m2_Ki = 0.5;
    const double m2_Kd = 0.1;
// storage variables
    // motor 1
    double m1_err_int = 0; 
    double m1_prev_err = 0;
    // motor 2
    double m2_err_int = 0;
    double m2_prev_err = 0;

// EMG calibration variables
double emg_gain1 = 1;       // set to one for unamplified signal
double emg_gain2 = 1;

double cal_samples = 25;
double normalize_emg_value = 1;      // set te desired value to calibrate the signal to

//// FILTER VARIABLES
// storage variables
        // differential action filter, same is used for both controllers
        double m_f_v1 = 0, m_f_v2 = 0;
        // input filter to smooth signal
        double r1_f_v1 = 0, r1_f_v2 = 0;
        double r2_f_v1 = 0, r2_f_v2 = 0;

// Filter coefficients
// differential action filter (lowpass 5Hz at 50samples)
    const double m_f_a1 = -1.1430, m_f_a2 = 0.4128, m_f_b0 = 0.0675, m_f_b1 = 0.1349, m_f_b2 = 0.0675;      // coefficients from sheets are used as first test.
// input filter (lowpass 1Hz at 50samples)
    const double r1_f_a1 = -1.6475, r1_f_a2 = 0.7009, r1_f_b0 = 0.0134, r1_f_b1 = 0.0267, r1_f_b2 = 0.0134;
    
// tweede orde notch filter 50 Hz
// biquad 1 coefficienten
    const double numnotch50biq1_1 = 1;
    const double numnotch50biq1_2 = -1.61816178466632;
    const double numnotch50biq1_3 = 1.00000006127058;
    const double dennotch50biq1_2 = -1.59325742941798;
    const double dennotch50biq1_3 = 0.982171881701431;
// biquad 2 coefficienten
    const double numnotch50biq2_1 = 1;
    const double numnotch50biq2_2 = -1.61816171933244;
    const double numnotch50biq2_3 = 0.999999938729428;
    const double dennotch50biq2_2 = -1.61431180968071;
    const double dennotch50biq2_3 = 0.982599066293075;
// highpass filter 20 Hz coefficienten
    const double numhigh20_1 = 0.837089190566345;
    const double numhigh20_2 = -1.67417838113269;
    const double numhigh20_3 = 0.837089190566345;
    const double denhigh20_2 = -1.64745998107698;
    const double denhigh20_3 = 0.700896781188403;
// lowpass 5 Hz coefficienten
    const double numlow5_1 =0.000944691843840162;
    const double numlow5_2 =0.00188938368768032;
    const double numlow5_3 =0.000944691843840162;
    const double denlow5_2 =-1.91119706742607;
    const double denlow5_3 =0.914975834801434;

// Define the storage variables and filter coeficients for eight filters
//filter 1
double f1_v1 = 0, f1_v2 = 0, f2_v1 = 0, f2_v2 = 0, f3_v1 = 0, f3_v2 = 0,f4_v1 = 0, f4_v2 = 0;
// filter2
double f1_v1t = 0, f1_v2t = 0, f2_v1t = 0, f2_v2t = 0, f3_v1t = 0, f3_v2t = 0,f4_v1t = 0, f4_v2t = 0;

////////////////////////////////////////////////////////////////
/////////////////// START OF SIDE FUNCTIONS ////////////////////
//////////////////////////////////////////////////////////////
// these functions are tailored to perform 1 specific function

// this funtion flips leds on and off accordin to input with 0 being on
void LED(int red,int green,int blue)
{
    ledred.write(red);
    ledgreen.write(green);
    ledblue.write(blue);
}    
    
// counts 2 radians
// this function takes counts from the encoder and converts it to the amount of radians from the zero position. 
// It has been set up for standard 2X DECODING!!!
double get_radians(double counts)       
{
    double radians = (counts/4200)*2*pi;        // 2X DECODING!!!!! ((32 counts/rotation, last warning)
    return radians;
}


// This functions takes a 0->1 input, uses passing by reference (&c_reference)
// to create a reference that moves with a variable speed. It is meant for 0->1 values
double reference_f(double input, double &c_reference, double limlow, double limhigh)
{
    double reference = c_reference + input * controlstep * Vmax ;
            // two if statements check if the reference exceeds the limits placed upon the arms
            if(reference < limlow){reference = limlow;}
            if(reference > limhigh){reference = limhigh;}
           c_reference = reference; // change the global variable to the latest location.
    return reference;
}


// This function takes the controller outputvalue and ensures it is between -1 and 1
// this is done to limit the motor input to possible values (the motor takes 0 to 1 and the sign changes the direction).
double outputlimiter (double output, double limit)
{
    if(output> limit)                                
    {
        output = 1;
    }
    else if(output < limit && output > 0)
    {
        output = output;
    }
    else if(output > -limit && output < 0)
    {
        output = output;
    }
    else if(output < -limit)
    {
        (output = -1);
    }
    return output;
}


// BIQUADFILTER CODE GIVEN IN SHEETS 
double biquadfilter(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;
    }

// biquadfilters die bij het filteren van signaal 2 horen, copy paste, alle waardes zijn veranderd naar +t (t van two of twee)
// (niet netjes maar werkt goed)
double biquadfiltert(double ut, double &v1t, double &v2t, const double a1t, const double a2t, const double b0t, const double b1t, const double b2t)
{
    double vt = ut- a1t*v1t-a2t*v2t;
    double yt = b0t*vt+b1t*v1t+b2t*v2t;
    v2t = v1t;
    v1t = vt;
    return yt;
}

// PID Controller given in sheets
// aangepast om zelfde filter te gebruiken en om de termen te splitsen
double PID(double e, const double Kp, const double Ki, const double Kd, double Ts,double &e_int, double &e_prev)
{
// Proportional
double P = Kp * e;
// Integral
e_int = e_int + Ts * e;
double I = e_int * Ki;
// Derivative   
double e_derr = (e - e_prev)/Ts;
e_derr = biquadfilter(e_derr, m_f_v1, m_f_v2, m_f_a1, m_f_a2, m_f_b0, m_f_b1, m_f_b2);
// 
e_prev = e;
double D = Kd* e_derr;
// PID
double output = P + I + D;
return output;
}

 double angle_limits(double phi, double limlow, double limhigh)
{
    if(phi < limlow)
    {
        phi = limlow;
    }
    if(phi > limhigh)
    {
        phi = limhigh;
    }
    return phi;
}

/////////////////////////////////////////////////////////////////////
////////////////// PRIMARY CONTROL FUNCTIONS ///////////////////////
///////////////////////////////////////////////////////////////////
// these functions are called by go-flags and are used to update main variables and send signals to motor

// function that updates the inputs
void EMG_filter()
{
// filteren van EMG signaal 1 (A0) eerst notch(2 biquads), dan highpass, rectify(abs()), lowpass
    double u1 = EMG_in.read();
    double y1 = biquadfilter( u1, f1_v1, f1_v2,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
    double y2 = biquadfilter( y1, f2_v1, f2_v2,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
    double y3 = biquadfilter( y2, f3_v1, f3_v2, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
    double y4 = abs(y3);
    double y5 = biquadfilter( y4, f4_v1, f4_v2, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
    // update global variables
    output1 = y5;
    output1_amp = y5*emg_gain1;   // update global variable

// filteren van EMG signaal 2 (A2), zelfde proces als signaal 1
    double u1t = EMG_int.read();
    double y1t = biquadfiltert( u1t, f1_v1t, f1_v2t,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
    double y2t = biquadfiltert( y1t, f2_v1t, f2_v2t,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
    double y3t = biquadfiltert( y2t, f3_v1t, f3_v2t, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
    double y4t = abs(y3t);
    double y5t = biquadfiltert( y4t, f4_v1t, f4_v2t, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
    // update global variables
    output2 = y5t;
    output2_amp = y5t*emg_gain2; 
    
    scope.set(0,output1_amp);
    scope.set(1,output2_amp);
    scope.send();
 }
 
 
 // function that updates the required motor angles
 void det_angles()
{
    
    
    if(output1>1) {output1 = 1;}
    if(output2>1) {output2 = 1;}
    
    output1 = potright.read();
    output2 = potleft.read();
    
    double xx = 50+output1_amp*20;
    
    double ymin = - sqrt(4900 - pow(xx,2));
    double ymax = sqrt(4900 - pow(xx,2));
    double yy = ymin+output2_amp*(ymax-ymin);
    double r = sqrt(pow(xx,2)+pow(yy,2)); // vector naar end effector
    double alfa = acos((2*pow(L,2)-pow(r,2))/(2*pow(L,2))); // alfa is de hoek tussen upper en lower arm
    double beta = acos((pow(r,2))/(2*L*r)); // beta is de hoek tussen upper arm en r
    
    double theta_one = (atan2(yy,xx)+beta); 
    double theta_two = (-pi + alfa); 
    
    double phi1 = 4*(theta_one) + 2.8;
    double phi2 = 4*(theta_one+theta_two) + 1.85;
    phi2 = -phi2;

    // check the input angles and apply the limits
    phi1 = angle_limits(phi1,limlow1,limhigh1);
    phi2 = angle_limits(phi2,limlow2,limhigh2);
    
    // smooth the input signal (lowpass 1Hz)    
    phi1 = biquadfilter(phi1, r1_f_v1, r1_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2);
    phi2 = biquadfilter(phi2, r2_f_v1, r2_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2);
    
    // write into global variables
    phi_one = phi1;
    phi_two = phi2;
    
    pc.printf("x = %f, y = %f, phi_one = %f, phi_two = %f \n",xx,yy,phi_one,phi_two);
}
// MOTOR 1
void motor1_control()
{
       
    double reference1 = phi_one;  
    
    // add smooth start up
    if(rc1 < start_loops)
    {
        rc1++;
        reference1 = ((double) rc1/start_loops)*reference1;
    }
    else
    {
        reference1 = reference1;
    }
    
    double rads1 = get_radians(motor1_enc.getPosition());    // determine the position of the motor
    double error1 = (reference1 - rads1);                       // determine the error (reference - position)
    double m_output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err);
    
    m_output1 = outputlimiter(m_output1,1); // relimit the output for safety
    if(m_output1 > 0) {                    // uses the calculated output to determine the direction  of the motor
        motor1_rich.write(0);
        motor1_aan.write(m_output1);
    } else if(m_output1 < 0) {
        motor1_rich.write(1);
        motor1_aan.write(abs(m_output1));
    }
}

// MOTOR 2
void motor2_control()
{
    double reference2 = phi_two;  
    
    if(rc2 < start_loops)
    {
        rc2++;
        reference2 = ((double) rc2/start_loops)*reference2;
    }
    else
    {
        reference2 = reference2;
    }    

    double rads2 = get_radians(motor2_enc.getPosition());    // determine the position of the motor
    double error2 = (reference2 - rads2);                       // determine the error (reference - position)
    double m_output2 = PID(error2, m2_Kp, m2_Ki, m2_Kd, controlstep, m2_err_int, m2_prev_err);
    
    m_output2 = outputlimiter(m_output2,1);
    if(m_output2 > 0) {                    // uses the calculated output to determine the direction  of the motor
        motor2_rich.write(0);
        motor2_aan.write(m_output2);
    } else if(m_output2 < 0) {
        motor2_rich.write(1);
        motor2_aan.write(abs(m_output2));
    }
}

// calibrate the emg-signal
// works bij taking a certain amount of samples adding them and then normalize to the desired value
void calibrate_amp()
{
    double total1 = 0;
    double total2 = 0;

    for(int i = 0; i<cal_samples; i++)
    {
        EMG_filter();       // run filter
        double input1 = output1;
        total1 = total1 + input1;       // sum inputs
        
        double input2 = output2;
        total2 = total2 + input2;
        wait(0.1);
    }
    emg_gain1 = normalize_emg_value/(total1/cal_samples);  // normalize the amplification so that the maximum signal hits the desired one
    emg_gain2 = normalize_emg_value/(total2/cal_samples);
   pc.printf("gain1 = %f, gain2 = %f",emg_gain1,emg_gain2);

}
//////////////////////////////////////////////////////////////////
//////////// DEFINE GO-FLAG FUNCTIONS ///////////////////////////
////////////////////////////////////////////////////////////////


void EMG_activate()
{ 
    emg_go = true; 
}
void angle_activate()
{ 
    cart_go = true; 
}
void motor1_activate()
{ 
    motor1_go = true; 
}
void motor2_activate()
{ 
    motor2_go = true; 
}

int main()
{
    pc.baud(115200);
    main_filter.attach(&EMG_activate, controlstep);
    cartesian.attach(&angle_activate, controlstep);
    controller1.attach(&motor1_activate, controlstep);      // call a go-flag
    controller2.attach(&motor2_activate, controlstep);   
    while(true) 
    {
        // button press functions
        // flow buttons
        if(buttonlinks.read() == 0)
        {
            loop_start = !loop_start;     
            wait(buttonlinks.read() == 1);
            wait(0.3);             
        }
        if(buttonrechts.read() == 0)
        {
            calib_start = !calib_start;     
            wait(buttonrechts.read() == 1);
            wait(0.3);             
        }
        // reverse buttons
        if(reverse_button_links.read() == 0)
        {
           reverse_links = !reverse_links;     
           wait(reverse_button_links.read() == 1);
           wait(0.3);             
        }
         if(reverse_button_rechts.read() == 0)
        {
            reverse_rechts = !reverse_rechts;     
            wait(reverse_button_rechts.read() == 1);
            wait(0.3);             
        }

        //////////////////////////////////////////////////
        // Main Control stuff and options
        if(loop_start == true && calib_start == false)        // check if start button = true then start the main control loops
        {
            LED(1,1,0); // turn blue led on
            if(cart_go)   { cart_go = false; det_angles();}
            if(emg_go)    { emg_go = false; EMG_filter();}
            if(motor1_go) { motor1_go = false; motor1_control();}
            if(motor2_go) { motor2_go = false; motor2_control();}
        }
        // shut off both motors 
        if(loop_start == false) {motor1_aan.write(0); motor2_aan.write(0);}
        
        // turn green led on // start calibration procedures
        if(loop_start == false && calib_start == true) 
        {   LED(1,0,1); 
            motor1_aan.write(0); 
            motor2_aan.write(0);
            calibrate_amp();        // 10 second calibration
            calib_start = false;    // turn fork off
        }
        
        // turn red led on
        if(loop_start == true && calib_start == true) 
        { 
        LED(0,1,1); 
        motor1_aan.write(0); 
        motor2_aan.write(0);
        // switch_type = !switch_type;
        }
        
        // turn leds off (both buttons false)
        else { LED(1,1,1);}
    }
}