File content as of revision 0:8f027052f23a:

#include "mbed.h"

// Load all the necessary pins, buttons, etc.

// Load all the EMGs
AnalogIn EMG1; // Move X
AnalogIn EMG2; // Move Y
AnalogIn EMG3; // Change direction

// Initiate motor 1
PwmOut pwmpin1(D6);
DigitalOut directionpin1(D7);
InterruptIn motor1A(D12);   // Encoder 1a
InterruptIn motor1B(D13);   // Encoder 1b
QEI         Encoder1(D12,D13,NC,64);    // Note that we use X2 encoding, so there are 4200 counts/rotation, as opposed to 8400 from the documentation.

// Initiate motor 2
DigitalOut directionpin2(D4);
PwmOut pwmpin2(D5);
InterruptIn motor2A(D10);   // Encoder 1a
InterruptIn motor2B(D11);   // Encoder 1b
QEI         Encoder2(D10,D11,NC,64);    // Note that we use X2 encoding, so there are 4200 counts/rotation, as opposed to 8400 from the documentation.

// -------------------------------------------------------------------------- \\

double FilterEMG1(EMG1){
    filter emg1
    return filteredemg1
}

double FilterEMG2(EMG2){
    filter emg2
    return filteredemg2
}

double FilterEMG3(EMG3){
    filter emg3
    return filteredemg3
}

// ----------------------Read & relate the emgs --------------------------- \\

// We want to know whether we should move X or Y
bool DirBool = 1; // initialise with moving X
void ChangeDir(emg3 lezen){
    // Read EMG 3
    if(voldoet emg3 aan de voorwaarde? verander dan de x/y bool.){
        DirBool = !DirBool;
        return DirBool;
}

// Start moving in the x-direction
void MoveX(bool DirBool, EMG1){
    // first, we check whether or not we should even move in the x-direction
    if(DirBool == 1){ // We can move in the x-direction if this is true
 // we make a list of 2 long, checking emg 1 and 2
 *list*
 check whether emg 1 is active or not
 if(emg1 voldoet aan statement){
     request a change in motor angle // could be done by saying motorangle = motorangle - something we want, check convention!!!
     }
     if(emg2 voldoet aan statement){
         request a change in motor angle // could be done by saying motorangle = motorangle - something we want, check convention!!!
         }
         return de angle die we willen veranderen voor de motoren
 }

// When we should move, we should relate this change in x-direction to a change 
// in the motor angles, this comes from the inverse kinematics.
 
 void MoveY(bool DirBool, EMG1){
    // first, we check whether or not we should even move in the y-direction
    if(DirBool == 0){ // We can move in the y-direction if this is true
 // we make a list of 2 long, checking emg 1 and 2
 *list*
 check whether emg 1 is active or not
 if(emg1 voldoet aan statement){
     request a change in motor angle // could be done by saying motorangle = motorangle - something we want, check convention!!!
     }
     if(emg2 voldoet aan statement){
         request a change in motor angle // could be done by saying motorangle = motorangle - something we want, check convention!!!
         }
         return de angle die we willen veranderen voor de motoren
 }
 
// When we should move, we should relate this change in y-direction to a change 
// in the motor angles, this comes from the inverse kinematics.

// ----------Set the desired motor positions & get the actual ones---------- \\

// Get position motor 1 should be in
double GetReferencePosition1(){
    return ReferencePosition1;
}

// Get position motor 1 actually is in
double GetActualPosition1(){
    double ActualPosition = Encoder1.getPulses(); // Gets the actual amount of counts from motor 1. Note the minus sign for convention (CCW = positive counts.)
    return ActualPosition1;
}

// Get position motor 2 should be in
double GetReferencePosition2(){
    return ReferencePosition2;
}

// Get position motor 2 actually is in
double GetActualPosition2(){
    double ActualPosition2 = Encoder2.getPulses(); // Gets the actual amount of counts from motor 1. Note the minus sign for convention (CCW = positive counts.)
    return ActualPosition2;
}

// PID controller for motor 1
double PID_controller1(double error){
    static double error_integral = 0;
    static double error_prev = error; // initialization with this value only done once!
    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
    double Ts = 0.01;
    
    // Proportional part
        
        double Kp = 0.1;
        double u_k = Kp*error;

    // Integral part
        double Ki = 0.1;
        error_integral = error_integral + error * Ts;
        double u_i = Ki * error_integral;
    
    // Derivative part
        double Kd = 10.1;
        double error_derivative = (error - error_prev)/Ts;
        double filtered_error_derivative = LowPassFilter.step(error_derivative);
        double u_d = Kd * filtered_error_derivative;
        error_prev = error;
    // Sum all parts and return it
    return u_k + u_i + u_d;
}

// PID controller for motor 2
double PID_controller2(double error){
    static double error_integral = 0;
    static double error_prev = error; // initialization with this value only done once!
    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
    double Ts = 0.01;
    
    // Proportional part
        
        double Kp = 0.1;
        double u_k = Kp*error;

    // Integral part
        double Ki = 0.1;
        error_integral = error_integral + error * Ts;
        double u_i = Ki * error_integral;
    
    // Derivative part
        double Kd = 10.1;
        double error_derivative = (error - error_prev)/Ts;
        double filtered_error_derivative = LowPassFilter.step(error_derivative);
        double u_d = Kd * filtered_error_derivative;
        error_prev = error;
    // Sum all parts and return it
    return u_k + u_i + u_d;
}

void SetMotor1(double motorValue){
    // Given -1<=motorValue<=1, this sets the PWM and direction
    // bits for motor 1. Positive value makes motor rotating
    // ccw. motorValues outside range are truncated to
    // within range
    if (motorValue >=0) directionpin1=1;
        else directionpin1=0;
    if (fabs(motorValue)>1) pwmpin1 = 1;
        else pwmpin1 = fabs(motorValue);
}

void SetMotor2(double motorValue){
    // Given -1<=motorValue<=1, this sets the PWM and direction
    // bits for motor 1. Positive value makes motor rotating
    // ccw. motorValues outside range are truncated to
    // within range
    if (motorValue >=0) directionpin2=1;
        else directionpin2=0;
    if (fabs(motorValue)>1) pwmpin2 = 1;
        else pwmpin2 = fabs(motorValue);
}

int main()
{
    while (true) {

    }
}