File content as of revision 28:aec0d9bdb949:

// 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);  /* This is not yet implemented! 
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(D11,D10,NC,4200);      // Counterclockwise motor rotation is the positive direction
QEI         Encoder2(D9,D8,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 double    dt = 0.5;
const double    Kp = 17.5;    // given value is 17.5
const double    Ki = 1.02;  // given value is 1.02
//const double    Ts = 0.0025; // Sample time in seconds

// Functions
    //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 > pi)
        {   angle = angle - 2.0*pi;
        }
        while (angle < pi)
        {   angle = angle + 2.0*pi;
        }
        return  angle;
    }
    
    double  ErrorCalc(double yref,double CurAngle)
    {   double  error = yref - CurAngle;
        return  error;
    }
            
    double  Pcontroller(double yref,double CurAngle)
    {   double  error = ErrorCalc(yref,CurAngle);
        //double Kp = 50.0*pot1;    // Normalised variable for value of potmeter 1
        // Proportional part:
        double  u_k = Kp * error;
        // Sum all parts and return it
        //pc.printf("error: %d",error);
        return  u_k;
    }

    double  PIcontroller(double yref,double CurAngle)
    {   double  error = yref - CurAngle;    
        static double   error_integral = 0; 
        // Proportional part:
        double  u_k = Kp * error;
        // Integral part
        error_integral = error_integral + error * dt;
        double  u_i = Ki * error_integral;
        // Sum all parts and return it
        return  u_k + u_i;
    }
    

void    turn1()    // main function, all below functions with an extra tab are called
{   
    double  refvalue1 = pi/4;
    int     counts1 = Counts1input();
    float   angle1 = CurrentAngle(counts1);
    double  PIcontr = PIcontroller(refvalue1,angle1);
    double  error = ErrorCalc(refvalue1,angle1);
     
    pin6 = fabs(PIcontr)/pi;
    if  (error > 0)
    {   pin7 = true;
    }
    else if (error < 0)
    {   pin7 = false;    
    }
    //pc.printf("%i\r\n",refvalue1);
    //pc.printf("Counts1,2,3: %i       Angle1,2,3: %f  \r\n",counts1,angle1);
}
   
/*double RefVelocity(float pot)
{   // Returns reference velocity in rad/s. 
    // Positive value means clockwise rotation.
    const float maxVelocity=8.4; // in rad/s of course!    
    double RefVel;  // in rad/s
    
    if (button1 == 1)   
    {   // Clockwise rotation      
        RefVel = pot*maxVelocity; 
    } 
    else
    {   // Counterclockwise rotation       
        RefVel = -1*pot*maxVelocity;   
    }
    return RefVel;
}
*/

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

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;
    pin5 = 0;
    pin6 = 0;
    exit (0);                       //Abort mission!!
}

// Main program
int main()
{   
    pc.baud(115200);  
    
    pin3.period(0.1);
    pin5.period(0.1);
    pin6.period(0.1);
    motor.attach(turn1,dt);

    emergencybutton.rise(Emergency);              //If the button is pressed, stop program
            
    while   (true)
    {   
        // Nothing here
    }
}