File content as of revision 0:0fd6a09e85d9:

#include "mbed.h"
#include "FastPWM.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "math.h"
#include "BiQuad.h"
#include "BiQuad4.h"
#include "FilterDesign.h"
#include "FilterDesign2.h"

// --------------------------------------------------
// ----------------- SET UP -------------------------
QEI Encoder1(D11, D10, NC, 4200) ;  // Encoder motor 1, (pin 1A, pin 1B, counts/rev)
QEI Encoder2(D9, D8, NC, 4200) ;    // Encoder motor 2, (pin 2A, pin 2B, counts/rev)
DigitalOut directionM1(D4);
DigitalOut directionM2(D7);
FastPWM motor1_pwm(D5);
FastPWM motor2_pwm(D6);
MODSERIAL pc(USBTX, USBRX);
DigitalOut ledr(LED_RED);
DigitalOut ledg(LED_GREEN);
DigitalOut ledb(LED_BLUE);
DigitalIn directionx(SW3); //x direction switch button
DigitalIn directiony(SW2); //y direction switch button
DigitalIn buttonx(D2); //x EMG replacement 
DigitalIn buttony(D3); //y EMG replacement
AnalogIn emg1_raw(A0);
AnalogIn emg2_raw(A1);

// Tickers
Ticker Demo;

// ---------------------------------------------------
// ----------------- GLOBAL VARIABLES ----------------
volatile int counts1; 
volatile int counts2;
volatile double theta1;
volatile double theta2;
const double  pi = 3.14159265359;
volatile double  error1;
volatile double  error2;
//volatile double  error1_final=10.0;
//volatile double  error2_final=10.0;
//volatile double q1_diff_final;
//volatile double q2_diff_final;
//double  point1x = 200.0;
//double  point1y = 200.0;
//double  point2x = 200.0;
//double  point2y = 100.0;
//double  point3x = 350.0;
//double  point3y = 0.0;
//double  point4x = 200.0;
//double  point4y = 0.0;
//volatile int track = 1;
const double x0 = 80.0; //zero x position after homing
const double y0 = 141.0; //zero y position after homing
volatile double  setpointx = x0;
volatile double  setpointy = y0;
volatile double  U1;
volatile double  U2;
volatile double xfinal;
volatile double yfinal;
volatile int sx;//value of the button and store as switch
volatile int sy;//value of the button and store as switch
double dirx = 1.0; //determine the direction of the setpoint placement
double diry = 1.0; //determine the direction of the setpoint placement
int need_to_move_1; // Does the robot needs to move in the first direction?
int need_to_move_2; // Does the robot needs to move in the second direction?
double EMG_calibrated_max_1 = 2.0000;  // Maximum value of the first EMG signal found in the calibration state.
double EMG_calibrated_max_2 = 2.0000;  // Maximum value of the second EMG signal found in the calibration state.

float threshold_EMG = 0.25;         // Threshold on 25 percent of the maximum EMG

double emg1_filtered = 0.00;
double emg2_filtered = 0.00;

const double v=0.1; //moving speed of setpoint 


// Inverse Kinematics
volatile double q1_diff;
volatile double q2_diff;
const double sq = 2.0; //to square numbers
const double L1 = 250.0; //length of the first link
const double L3 = 350.0; //length of the second link

// Reference angles of the starting position
double q2_0 = -acos((pow(x0,sq)+pow(y0,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3));
double q1_0 = atan(y0/x0)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x0,sq)+pow(y0,sq))/(2.0*L1*sqrt(pow(x0,sq)+pow(y0,sq))));
double q2_0_enc = (pi-q2_0) - q1_0;

// --------------------------------------------------------------------
// -------------- Get EMG values --------------------------------------
// --------------------------------------------------------------------

// Sample: samples the EMG and sends it to HIDScope
void sample()
{
    emg1_filtered = FilterDesign(emg1_raw.read());
    emg2_filtered = FilterDesign2(emg2_raw.read());
    
    //scope.set(0, emg1_raw.read());      // Raw EMG 1 send to scope 0
    //scope.set(1, emg1_filtered);        // Filtered EMG 1 send to scope 1
    //scope.set(2, emg2_raw.read());      // Raw EMG 2 send to scope 2
    //scope.set(3, emg2_filtered);        // Filtered EMG 2 send to scope 3
    //scope.send();                       // Send the data to the computer
}


// --------------------------------------------------------------------
// ---------------Read out encoders------------------------------------
// --------------------------------------------------------------------    
double  counts2angle1()
{
    counts1 = Encoder1.getPulses();         // Counts of outputshaft of motor 1
    theta1 = -(double(counts1)/4200) * 2*pi;       // Angle of outputshaft of motor 1
    return theta1;
}

double counts2angle2()
{
    counts2 = Encoder2.getPulses();         // Counts of outputshaft of motor 2
    theta2 = (double(counts2)/4200) * 2*pi;       // Angle of outputshaft of motor 2
    return theta2;
}

// -------------------------------------------------------------------------
// -------------- Determine Setpoints --------------------------------------
// -------------------------------------------------------------------------

void determinedemoset(){
    setpointx = setpointx + dirx*need_to_move_1*v;
    setpointy = setpointy + diry*need_to_move_2*v;
    }

//function to change the moving direction of the setpoint
void ChangeDirectionX(){
    dirx = -1*dirx;
    }
    
void ChangeDirectionY(){
    diry = -1*diry;
    }    



// -----------------------------------------------------------------    
// --------------------------- PI controllers ----------------------
// -----------------------------------------------------------------
double PI_controller1(double error1)
{
    static double error_integral1 = 0;
    
    // Proportional part
    double Kp1 = 3.95;              // Kp (proportionele controller, nu nog een random waarde)
    double u_p1 = Kp1*error1;        // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
    
    // Integral part
    double Ki1 = 6.0;             // Ki (Integrale deel vd controller, nu nog een random waarde)
    double Ts1 = 0.005;           // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine)   
    error_integral1 = error_integral1 + error1 * Ts1;  
    double u_i1 = Ki1 * error_integral1;
    
    // Sum 
    U1 = u_p1 + u_i1;
    
    // Return
    return U1;      
}
double PI_controller2(double error2)
{
    static double error_integral2 = 0;
    
    // Proportional part
    double Kp2 = 3.95;              // Kp (proportionele controller, nu nog een random waarde)
    double u_p2 = Kp2*error2;        // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
    
    // Integral part
    double Ki2 = 6.0;             // Ki (Integrale deel vd controller, nu nog een random waarde)
    double Ts2 = 0.005;           // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine)   
    error_integral2 = error_integral2 + error2 * Ts2;  
    double u_i2 = Ki2 * error_integral2;
    
    // Sum +
    U2 = u_p2 + u_i2;
    
    // Return
    return U2;      
} 

// ------------------------------------------------------------
// ------------ Inverse Kinematics ----------------------------
// ------------------------------------------------------------
double makeAngleq1(double x, double y){
    double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2.0*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration
    q1_diff = -(2.0*(q1-q1_0));                                                                                    //the actual amount of radians that the motor has to turn in total to reach the setpoint
    return q1_diff;
}

double makeAngleq2(double x, double y){
    double q2 = -acos((pow(x,sq)+pow(y,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3)); //angle of the second joint in setpoint configuration
    double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2.0*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration
    double q2_motor = (pi - q2) - q1; //because q2 represents the angle at joint two and not at the motor a calculation has to be done
    q2_diff = (2.0*(q2_motor - q2_0_enc)); //the actual amount of radians that the motor has to turn in total to reach the setpoint
    return q2_diff;
}


// -----------------------------------------------
// ------------ RUN MOTORS -----------------------
// -----------------------------------------------
void motoraansturing()
{
    sample();
    determinedemoset();
    q1_diff = makeAngleq1(setpointx, setpointy);
    q2_diff = makeAngleq2(setpointx, setpointy);
    //q1_diff_final = makeAngleq1(xfinal, yfinal);
    //q2_diff_final = makeAngleq2(xfinal, yfinal);

    theta2 = counts2angle2();           
    error2 = q2_diff - theta2;
    theta1 = counts2angle1();  
    error1 = q1_diff - theta1;          // Setpoint error, te behalen setpoint minus de huidige positie van de as.
    
    //errors die de setpoints bepalen
    //error1_final = q1_diff_final - theta1;
    //error2_final = q2_diff_final - theta2;

    U1 = PI_controller1(error1);       // Voltage dat naar de motor gestuurd wordt. 
    U2 = PI_controller2(error2);   
    
    motor1_pwm.write(fabs(U1));         // Motor aansturen
    directionM1 = U1 > 0.0f;            // Richting van de motor bepalen
    motor2_pwm.write(fabs(U2));
    directionM2 = U2 > 0.0f;     
}


void rundemo()
{
    motoraansturing();
}


int main()
{
    pc.baud(115200);
    motor1_pwm.period_us(60);                // Period is 60 microseconde
    motor2_pwm.period_us(60);
    Demo.attach(&rundemo, 0.005f); 
    
    InterruptIn directionx(SW3);
    directionx.fall(ChangeDirectionX); //change the direction of the setpoint in x direction
    InterruptIn directiony(SW2);
    directiony.fall(ChangeDirectionY); //change the direction of the setpoint in y direction
    
    pc.printf("\r\n\r\nDOE HET AUB!!! \r\n\r\n");
    
    while (true) {
        //sx = !buttonx.read(); //this has to be replaced with the input from the EMG, this then functions like a button
        //sy = !buttony.read(); //this has to be replaced with the input from the EMG, this then functions like a button
        if (emg1_filtered >= (threshold_EMG*EMG_calibrated_max_1)){
            need_to_move_1 = 1; // The robot does have to move
            }
        else {
            need_to_move_1 = 0; // If the robot does not have to move
            }

        if(emg2_filtered >= threshold_EMG*EMG_calibrated_max_2){
            need_to_move_2 = 1;
            }
        else {
            need_to_move_2 = 0;
            }
        pc.printf("Setpointx: %0.2f, Setpointy: %02f, q1_diff: %0.2f, q2_diff: %0.2f, error1: %0.2f, error2: %0.2f, U1: %0.2f, U2: %0.2f, movex: %i, movey: %i\r\n", setpointx,setpointy,q1_diff,q2_diff,error1,error2,U1,U2,need_to_move_1,need_to_move_2);
        
        wait(0.5f);
   
    }
}