File content as of revision 0:5502fbcdba50:

#include "mbed.h"
    
double GetActualPosition1(){
    // Gets the actual position from motor 1 in rad. The difference with the
    // reference position is the error we are going to use in  the PID-control.
    double ActualPosition = Encoder1.getPulses()/4200*2*3.14159265358979; 
    return ActualPosition;
}

double GetReferencePosition1(double ActualPosition, double DesiredChange){
        ReferencePosition1 = ActualPosition + DesiredChange; // Gives the position it should be in.
    return ReferencePosition1;
}

double GetReferencePosition2(double ActualPosition, double DesiredChange){
    MotAng2 = IK();
    ReferencePosition2 = ActualPosition + DesiredChange; // Gives the position it should be in.
    return ReferencePosition2;
}
    
double GetActualPosition2(){
    double ActualPosition = Encoder2.getPulses()/4200*2*3.14159265358979;
    return ActualPosition;
}

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

// 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
    // clockwise. motorValues outside range are truncated to
    // within range. Motor value = error from the PID controller.
    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 2. Positive value makes motor rotating
    // clockwise. 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);
}

void do_state_movingX(double EMG1, double EMG2){
    // EMG 1 and 2 are being read at 100 Hz, which we will use. Script
    // checks whether it should move negative, positive or not at all.
    
    // When EMG1 is on, it should move in the -X direction.
    if(EMG1==1){
        
        // Get the position of motor 1 and get the wanted position, where the
        // difference is the error for our PID controllers.
        AP1 = GetActualPosition1();
        DesiredChange = IK(-X velocity);
        RP1 = GetReferencePosition1(double AP1, DesiredChange);
        pwm1 = double PID1(RP1-AP1);
        SetMotor1(pwm1);

        }
    if(EMG2==1){
        // Same for motor 2.
        AP2 = GetActualPosition1();
        DesiredChange = IK(+X velocity);
        RP2 = GetReferencePosition1(double AP1, DesiredChange);
        pwm2 = double PID2(RP2-AP2);    
        SetMotor2(pwm2);
}
int main()
{
    while (true) 
    }
}