File content as of revision 2:867a1eb33bbe:

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

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);

int reference1 = 0;         // set the reference position of the encoders (not used)
int reference2 = 0;

////////
// 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
double output1;
double output2;

double phi_one;
double phi_two;


// EXTRA INPUTS AND REQUIRED VARIABLES
//POTMETERS
    AnalogIn potright(A0);      // define the potmeter outputpins
    AnalogIn potleft(A1);
    
// Analoge input signalen defineren
AnalogIn    EMG_in(A2);                 // EMG_in.read kan je nu gebruiken om het analoge signaal A2 uit te lezen
AnalogIn    EMG_int(A3);                // 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 signal amplification  (needed with EMG, used in control loop, precise amp determination is a work in progress!!!!)    ??
    double signalamp1 = 1;
    double signalamp2 = 1;
 // Define storage variables for reference values
    double c_reference1 = 0;
    double c_reference2 = 0;
// limit  angles (in radians)
    // motor1
    const double limlow1 = 0.5;             // min height
    const double limhigh1 = 4.5;            // max height
    // motor 2
    const double limlow2 = -4.5;            // maximum height, motor has been inverted due to transmission
    const double limhigh2 = 2;              // minimum height
    
// 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;


//// 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).
// needs more work to use it for the wind-up prevention.
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)
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 later gesplitst te kunnen limiteren (windup preventie!!)
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;
}
/////////////////////////////////////////////////////////////////////
////////////////// PRIMARY CONTROL FUNCTIONS ///////////////////////
///////////////////////////////////////////////////////////////////
// these functions are called by go-flags and are used to update main variables and send signals to motor

// MOTOR 1
void motor1_control()
{
    //double m_input1 = potright.read(); // used for pot input
    //input1 = 0.4505;
    
    // first input edit (limit signal between threshold and 1, and reverse if wanted
    //if(m_input1 < input_threshold) {m_input1 = 0;}
    //if(m_input1 > 1) {m_input1 = 1;}
    //if(reverse_rechts == true) {m_input1 = -m_input1;}
   // m_input1 = biquadfilter(m_input1, r1_f_v1, r1_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2);        //biquad with diff-filter  coefficients to smooth input
    
    
    //double reference1 = reference_f(m_input1, c_reference1,limlow1,limhigh1);      // determine the reference that has been set by the inputsignal
    //scope.set(0,reference1);
    double reference1 = phi_one;
    scope.set(0,reference1);
    pc.printf("ref1 = %f",reference1);
    double rads1 = get_radians(motor1_enc.getPosition());    // determine the position of the motor
    //scope.set(1,rads1);
    pc.printf("rads1 = %f ",rads1);
    //scope.send();
    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 m_input2 = potleft.read()*signalamp2;      // replace potleft with filter
    
    // first input limiter
    //if(m_input2 < input_threshold) {m_input2 = 0;}
    //if(m_input2 > 1) {m_input2 = 1;}
    //if(reverse_links == false) {m_input2 = -m_input2;}
    //m_input2 = biquadfilter(m_input2, r2_f_v1, r2_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2);
    
    
    //double reference2 = reference_f(m_input2, c_reference2,limlow2,limhigh2);      // determine the reference that has been set by the clamped inputsignal
    
    double reference2 = phi_two;
    scope.set(1,reference2);
    scope.send();
    double rads2 = get_radians(motor2_enc.getPosition());    // determine the position of the motor
    pc.printf("rads2 = %f ",rads2);
    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));
    }
}


// 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);
    output1 = y5;    // update global variable
// versturen van het input signaal u1 en het gefilterde signaal y5 naar HIDScope channel 0 en 1
    //scope.set(0,u1);
    //scope.set(1,y5);
// 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);
    output2 = y5t; // update global variable
    
    // versturen van input signaal2; u1t en het gefilterde signaal; y5t naar HIDScope channel 2 en 3
    //scope.set(2,u1t);
    //scope.set(3,y5t);
    // verzenden van de verstuurde signalen
    //scope.send();
 }
 
 // function that updates the required motor angles
 void det_angles()
{
    output1 = potright.read();
    output2 = potleft.read();
    double xx = 66+output1*4;
    double yy = -16+output2*32;
    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
    // hoeken berekenen
    //pc.printf("xx = %f ",xx);
    //pc.printf("yy = %f    ",yy);
    //scope.set(0,xx);
    //scope.set(1,yy);
    
    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;
    if(phi1 < limlow1){phi1 = limlow1;}
    if(phi1 > limhigh1){phi1 = limhigh1;}
    
    if(phi2 < limlow2){phi2 = limlow2;}           // reverse limlow2 limhigh2 becasue they are inverse due to transmission 
    if(phi2 > limhigh2){phi2 = limhigh2;}
    
    phi_one = phi1;
    phi_two = phi2;
    
    //theta_one = theta_one*4 ;
    //theta_two = theta_two*4 ;
    

    
    //pc.printf("theta1 = %f ",theta_one);
    //pc.printf("theta2 = %f \n",theta_two);
    //scope.set(2,phi1);
    //scope.set(3,phi2);
    
    //scope.send();
    // testen van de functie
    double xt = L*cos(theta_one)+L*cos(theta_one+theta_two);
    double yt = L*sin(theta_one)+L*sin(theta_one+theta_two);

}
//////////////////////////////////////////////////////////////////
//////////// 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);}
        
        // turn red led on
        if(loop_start == true && calib_start == true) { LED(0,1,1); motor1_aan.write(0); motor2_aan.write(0);}
        
        // turn leds off (both buttons false)
        else { LED(1,1,1);}
    }
}