File content as of revision 49:48363ca21a15:

// Inclusion of libraries
#include    "mbed.h"
#include    "FastPWM.h"    
#include    "QEI.h"        // Includes library for encoder
#include    "MODSERIAL.h"
#include    "HIDScope.h"
#include    "BiQuad.h"

// Input
AnalogIn    pot1(A1);
AnalogIn    pot2(A2);
InterruptIn button1(D0);
InterruptIn button2(D1);
InterruptIn emergencybutton(SW2);   //The button SW2 on the K64F is the emergency button: if you press this, everything will abort as soon as possible

DigitalIn   pin8(D8);       // Encoder 1 B
DigitalIn   pin9(D9);       // Encoder 1 A
DigitalIn   pin10(D10);     // Encoder 2 B
DigitalIn   pin11(D11);     // Encoder 2 A
DigitalIn   pin12(D12);     // Encoder 3 B
DigitalIn   pin13(D13);     // Encoder 3 A

// Output
DigitalOut  pin2(D2);       // Motor 3 direction = motor flip
FastPWM     pin3(A5);       // Motor 3 pwm = motor flip
DigitalOut  pin4(D4);       // Motor 2 direction = motor right
FastPWM     pin5(D5);       // Motor 2 pwm = motor right
FastPWM     pin6(D6);       // Motor 1 pwm = motor left
DigitalOut  pin7(D7);       // Motor 1 direction = motor left

DigitalOut  greenled(LED_GREEN,1);
DigitalOut  redled(LED_RED,1);
DigitalOut  blueled(LED_BLUE,1);

// Utilisation of libraries
MODSERIAL   pc(USBTX, USBRX);
QEI         Encoderl(D9,D8,NC,4200);        // Counterclockwise motor rotation is the positive direction
QEI         Encoderr(D10,D11,NC,4200);      // Counterclockwise motor rotation is the positive direction
QEI         Encoderf(D12,D13,NC,4200);      // Counterclockwise motor rotation is the positive direction
Ticker      motorl;
Ticker      motorr;
Ticker      motorf;
Ticker      encoders;

// Global variables
const float     pi = 3.14159265358979f;
float           u3 = 0.0f;         // Normalised variable for the movement of motor 3
const float     fCountsRad = 4200.0f;
const float     dt = 0.002f;

float   currentanglel;
float   errorl;
float   currentangler;
float   errorr;
float   currentanglef;
float   errorf;
float   Kp;
float   Kd;

// Functions
void Emergency()        // Emgergency, if SW2 on biorobotics is pressed, everything will instantly abort and a red light goes on
{   greenled = 1;       // Red light on
    blueled = 1;
    redled = 0;
    pin3 = 0;           // All motor forces to zero
    pin5 = 0;
    pin6 = 0;
    exit (0);           // Abort mission!!
}

    // Subfunctions
    int Countslinput()      // Gets the counts from encoder 1
    {   int     countsl;
        countsl = Encoderl.getPulses();
        return  countsl;
    }
    int Countsrinput()      // Gets the counts from encoder 2
    {   int     countsr;
        countsr = Encoderr.getPulses();
        return  countsr;
    }
    int Countsfinput()      // Gets the counts from encoder 3
    {   int     countsf;
        countsf = Encoderf.getPulses();
        return  countsf;
    }

    float   CurrentAngle(float counts)      // Calculates the current angle of the motor (between -2*pi to 2*pi) based on the counts from the encoder
    {   float   angle = ((float)counts*2.0f*pi)/fCountsRad;
        while   (angle > 2.0f*pi)
        {   angle = angle-2.0f*pi;
        }
        while   (angle < -2.0f*pi)
        {   angle = angle+2.0f*pi;
        }
        return angle;
    }

    float  ErrorCalc(float refvalue,float CurAngle)     // Calculates the error of the system, based on the current angle and the reference value
    {   float   error = refvalue - CurAngle;
        return  error;
    }
        
        float Kpcontr()     // Sets the Kp value for the controller dependent on the scaled angle of potmeter 2
        {   float   Kp = 40.0f*pot2;
            return  Kp;
        }
        
        float Kdcontr()     // Sets the Kd value for the controller dependent on the scaled angle of potmeter 1
        {   float   Kd = 10.0f*pot1;
            return  Kd;
        }               
                   
    float PIDcontrollerl(float refvalue,float CurAngle)  // PID controller for the motors, based on the reference value and the current angle of the motor
    {   Kp = Kpcontr();
        //float   Kp = 10.42f; 
        float Ki = 1.02f;
        //float   Kd = 0.0493f;
        Kd = Kdcontr();
        float   error = ErrorCalc(refvalue,CurAngle);
        static float    error_integrall = 0.0;
        static float    error_prevl = error; // initialization with this value only done once!
        static  BiQuad  PIDfilterl(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
        // Proportional part:
        float   u_k = Kp * error;
        // Integral part
        error_integrall = error_integrall + error * dt;
        float   u_i = Ki * error_integrall;
        // Derivative part
        float   error_derivative = (error - error_prevl)/dt;
        float   filtered_error_derivative = PIDfilterl.step(error_derivative);
        float   u_d = Kd * filtered_error_derivative;
        error_prevl = error;
        // Sum all parts and return it
        return  u_k + u_i + u_d;
    }

    float DesiredAnglel()        // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
    {   float   angle = (pot1*2.0f*pi)-pi;
        return  angle;
    }    
        
void    turnl()    // main function for movement of motor 1, all above functions with an extra tab are called
{   
    float   refvaluel = 0.5f*pi;
    //float   refvaluel = DesiredAnglel();                             // different minus sign per motor
    int     countsl = Countslinput();                               // different encoder pins per motor
    currentanglel = CurrentAngle(countsl);                  // different minus sign per motor
    float   PIDcontroll = PIDcontrollerl(refvaluel,currentanglel);   // same for every motor
    errorl = ErrorCalc(refvaluel,currentanglel);            // same for every motor
    
    pin6 = fabs(PIDcontroll);                                       // different pins for every motor
    pin7 = PIDcontroll > 0.0f;                                      // different pins for every motor
}

    float PIDcontrollerr(float refvalue,float CurAngle)  // PID controller for the motors, based on the reference value and the current angle of the motor
    {   //float   Kp = Kpcontr();
        float   Kp = 10.42f; 
        float   Ki = 1.02f;
        float   Kd = 0.0493f;
        //float   Kd = Kdcontr();
        float   error = ErrorCalc(refvalue,CurAngle);
        static float    error_integralr = 0.0;
        static float    error_prevr = error; // initialization with this value only done once!
        static  BiQuad  PIDfilterr(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
        // Proportional part:
        float   u_k = Kp * error;
        // Integral part
        error_integralr = error_integralr + error * dt;
        float   u_i = Ki * error_integralr;
        // Derivative part
        float   error_derivative = (error - error_prevr)/dt;
        float   filtered_error_derivative = PIDfilterr.step(error_derivative);
        float   u_d = Kd * filtered_error_derivative;
        error_prevr = error;
        // Sum all parts and return it
        return  u_k + u_i + u_d;
    }
    
    float DesiredAngler()        // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
    {   float   angle = (pot2*2.0f*pi)-pi;
        return  angle;
    }  

void    turnr()    // main function for movement of motor 1, all above functions with an extra tab are called
{   
    //float   refvaluer = pi/4.0f;
    float   refvaluer = DesiredAngler();                             // different minus sign per motor
    int     countsr = Countsrinput();                               // different encoder pins per motor
    currentangler = CurrentAngle(countsr);                  // different minus sign per motor
    float   PIDcontrolr = PIDcontrollerr(refvaluer,currentangler);   // same for every motor
    errorr = ErrorCalc(refvaluer,currentangler);            // same for every motor
    
    pin5 = fabs(PIDcontrolr);                                       // different pins for every motor
    pin4 = PIDcontrolr > 0.0f;                                      // different pins for every motor
}

    float PIDcontrollerf(float refvalue,float CurAngle)  // PID controller for the motors, based on the reference value and the current angle of the motor
    {   //float   Kp = Kpcontr();
        float   Kp = 10.42f; 
        float   Ki = 1.02f;
        float   Kd = 0.0493f;
        //float   Kd = Kdcontr();
        float   error = ErrorCalc(refvalue,CurAngle);
        static float    error_integralf = 0.0;
        static float    error_prevf = error; // initialization with this value only done once!
        static  BiQuad  PIDfilterf(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
        // Proportional part:
        float   u_k = Kp * error;
        // Integral part
        error_integralf = error_integralf + error * dt;
        float   u_i = Ki * error_integralf;
        // Derivative part
        float   error_derivative = (error - error_prevf)/dt;
        float   filtered_error_derivative = PIDfilterf.step(error_derivative);
        float   u_d = Kd * filtered_error_derivative;
        error_prevf = error;
        // Sum all parts and return it
        return  u_k + u_i + u_d;
    }
    
    float DesiredAnglef()        // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
    {   float   angle = (pot2*2.0f*pi)-pi;
        return  angle;
    }  

void    turnf()    // main function for movement of motor 1, all above functions with an extra tab are called
{   
    //float   refvaluef = pi/4.0f;
    float   refvaluef = -DesiredAnglef();                             // different minus sign per motor
    int     countsf = Countsfinput();                               // different encoder pins per motor
    currentanglef = CurrentAngle(countsf);                  // different minus sign per motor
    float   PIDcontrolf = PIDcontrollerf(refvaluef,currentanglef);   // same for every motor
    errorf = ErrorCalc(refvaluef,currentanglef);            // same for every motor
    
    pin3 = fabs(PIDcontrolf);                                       // different pins for every motor
    pin2 = PIDcontrolf > 0.0f;                                      // different pins for every motor
}

float ActualPosition(int counts, int offsetcounts)      // After calibration, this function is used to return the counts (and thus the angle of the system) to 0
{   float MotorPosition = - (counts - offsetcounts) / fCountsRad;
    // minus sign to correct for direction convention
    return MotorPosition;
}

// Main program
int main()
{   
    pc.baud(115200);  
    pin3.period_us(15);     // If you give a period on one pin, c++ gives all pins this period
        
    motorl.attach(turnl,dt);
    //motorr.attach(turnr,dt);
    //motorf.attach(turnf,dt);
    
    emergencybutton.rise(Emergency);              //If the button is pressed, stop program
                
    while   (true)
    {   
        //pc.printf("angle l/r/f: \t %f \t\t %f \t\t %f \t\t error l/r/f: \t %f \t\t %f \t\t %f\r\n",currentanglel,currentangler,currentanglef,errorl,errorr,errorf);
        
        pc.printf("Kp: %f \t\t Kd: %f \t\t angle: %f \t\t error: %f\r\n",Kp,Kd,currentanglel,errorl);
        wait(0.1f);   
    }
}