File content as of revision 31:91ad5b188bd9:

// 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.14159265358979;
float           u3 = 0.0;         // Normalised variable for the movement of motor 3
const float     fCountsRad = 4200.0;
const float     dt = 0.001;

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

    // Subfunctions
    int Counts1input()
    {   int     counts1;
        counts1 = Encoder1.getPulses();
        return  counts1;
    }
    int Counts2input()
    {   int     counts2;
        counts2 = Encoder2.getPulses();
        return  counts2;
    }
    int Counts3input()
    {   int     counts3;
        counts3 = Encoder3.getPulses();
        return  counts3;
    }

    float   CurrentAngle(float counts)
    {   float   angle = ((float)counts*2.0*pi)/fCountsRad;
        while   (angle > 2.0*pi)
        {   angle = angle-2.0*pi;
        }
        while   (angle < -2.0*pi)
        {   angle = angle+2.0*pi;
        }
        return angle;
    }

    float  ErrorCalc(float yref,float CurAngle)
    {   float   error = yref - CurAngle;
        return  error;
    }
        
        float Kpcontr()
        {   float   Kp = 20*pot2;
            return  Kp;
        }
        
        float Kdcontr()
        {   float   Kd = 0.25*pot1;
            return  Kd;
        }               
               
    float PIDcontroller(float yref,float CurAngle)
    {   //float   Kp = Kpcontr();
        float   Kp = 10.42; 
        float   Ki = 1.02;
        float   Kd = 0.0493;
        //float   Kd = Kdcontr();
        float   error = ErrorCalc(yref,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()
        {   float   angle = (pot1*2.0*pi)-pi;
            return  angle;
        }

void    turn1()    // main function, all below functions with an extra tab are called
{   
    //float   refvalue1 = pi/4.0;
    float   refvalue1 = DesiredAngle();
    int     counts1 = Counts1input();
    float   angle1 = CurrentAngle(counts1);
    float   PIDcontr = PIDcontroller(refvalue1,angle1);
    float   error = ErrorCalc(refvalue1,angle1);
    
    pin6 = fabs(PIDcontr);
    pin7 = PIDcontr > 0.0f;
    //pin6 = 0.4+0.6*fabs(PIDcontr);     //geschaald
    //pin6 = fabs(PIDcontr)/pi;
    pc.printf("     error:  %f      ref: %f     angle:  %f \r\n",error,refvalue1,angle1);
}

float ActualPosition(int counts, int offsetcounts)
{   // After calibration, the counts are returned to 0;
    float MotorPos = - (counts - offsetcounts) / fCountsRad;
    // minus sign to correct for direction convention
    return MotorPos;
}

// Main program
int main()
{   
    pc.baud(115200);  
    pin3.period_us(15);     // Period on one pin, gives period on all pins
        
    //motor.attach(turn1,dt);

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