File content as of revision 32:a5b411833d1e:

// 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
FastPWM     pin3(D3);       // Motor 3 pwm
DigitalOut  pin4(D4);       // Motor 2 direction
FastPWM     pin5(D5);       // Motor 2 pwm
FastPWM     pin6(D6);       // Motor 1 pwm
DigitalOut  pin7(D7);       // Motor 1 direction

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

// Utilisation of libraries
MODSERIAL   pc(USBTX, USBRX);
QEI         Encoder1(D9,D8,NC,4200);        // Counterclockwise motor rotation is the positive direction
QEI         Encoder2(D11,D10,NC,4200);      // Counterclockwise motor rotation is the positive direction
QEI         Encoder3(D13,D12,NC,4200);      // Counterclockwise motor rotation is the positive direction
Ticker      motor;
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.001f;

// 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 Counts1input()      // Gets the counts from encoder 1
    {   int     counts1;
        counts1 = Encoder1.getPulses();
        return  counts1;
    }
    int Counts2input()      // Gets the counts from encoder 2
    {   int     counts2;
        counts2 = Encoder2.getPulses();
        return  counts2;
    }
    int Counts3input()      // Gets the counts from encoder3
    {   int     counts3;
        counts3 = Encoder3.getPulses();
        return  counts3;
    }

    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 = 20.0f*pot2;
            return  Kp;
        }
        
        float Kdcontr()     // Sets the Kd value for the controller dependent on the scaled angle of potmeter 1
        {   float   Kd = 0.25f*pot1;
            return  Kd;
        }               
               
    float PIDcontroller(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_integral = 0.0;
        static float    error_prev = error; // initialization with this value only done once!
        static  BiQuad  PIDfilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
        // Proportional part:
        float   u_k = Kp * error;
        // Integral part
        error_integral = error_integral + error * dt;
        float   u_i = Ki * error_integral;
        // Derivative part
        float   error_derivative = (error - error_prev)/dt;
        float   filtered_error_derivative = PIDfilter.step(error_derivative);
        float   u_d = Kd * filtered_error_derivative;
        error_prev = error;
        // Sum all parts and return it
        pc.printf ("Kp = %f     Kd = %f",Kp,Kd);
        return  u_k + u_i + u_d;
    }

        float DesiredAngle()        // 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    turn1()    // main function for movement of motor 1, all above functions with an extra tab are called
{   
    //float   refvalue1 = pi/4.0;
    float   refvalue1 = DesiredAngle();
    int     counts1 = Counts1input();
    float   currentangle1 = CurrentAngle(counts1);
    float   PIDcontrol1 = PIDcontroller(refvalue1,currentangle1);
    float   error1 = ErrorCalc(refvalue1,currentangle1);
    
    pin6 = fabs(PIDcontrol1);
    pin7 = PIDcontrol1 > 0.0f;
    //pin6 = 0.4+0.6*fabs(PIDcontr);     //geschaald
    //pin6 = fabs(PIDcontr)/pi;
    pc.printf("     error:  %f      ref: %f     angle:  %f \r\n",error1,refvalue1,currentangle1);
}

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
        
    //motor.attach(turn1,dt);

    emergencybutton.rise(Emergency);              //If the button is pressed, stop program
    //pin6 = 0.01; 
            
    while   (true)
    {   
        turn1();
        wait(dt);    
    }
}