File content as of revision 23:0c02cf961344:

// 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
//float u1  = pot1;

// 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      encoder;

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


// Functions
/*float   AngleCalc(float counts)
{   float angle = ((float)counts*2.0*pi)/fCountsRad;
    /*if (angle > 2.0*pi)
    {   angle = angle - 2.0*pi;
    }
    else if (angle < 2.0*pi)
    {   angle = angle + 2.0*pi;
    }
    
    return angle;
}
*/

void Encoderinput()
{   int counts1;
    int counts2;
    int counts3;
    float  angle1;
    float  angle2;
    float  angle3;
    counts1 = Encoder1.getPulses();
    counts2 = Encoder2.getPulses();
    counts3 = Encoder3.getPulses();
    //angle1 = AngleCalc (counts1);
    //angle2 = AngleCalc (counts2);
    //angle3 = AngleCalc (counts3);
    angle1 = ((float)counts1*2.0*pi)/fCountsRad;
    angle2 = ((float)counts2*2.0*pi)/fCountsRad;
             
    pc.printf("Counts1,2,3: %i  %i  %i  Angle1,2,3: %f  %f  %f  \r\n",counts1,counts2,counts3,angle1,angle2,angle3);
}

/*double RefVelocity()
{   // Returns reference velocity in rad/s. 
    // Positive value means clockwise rotation.
    const float maxVelocity=8.4; // in rad/s of course!    
    double RefVelocity;  // in rad/s
    
    if (button1 == 1)   
    {   // Clockwise rotation      
        RefVelocity = potMeterIn * maxVelocity; 
    } 
    else
    {   // Counterclockwise rotation       
        RefVelocity = -1*potMeterIn * maxVelocity;   
    }
    return RefVelocity;
}

double ActualPosition()
{   
    double MotorPos = - (counts1 - offsetcounts1) / dCountsRad;
    // minus sign to correct for direction convention
    return MotorPos;
}
*/
    
void turn()    
/*  Function for the movement of all motors, using the potmeters for the moving
    direction and speed of motor 1 and 2, and using button 1 and 2 on the biorobotics
    shield for the moving direction and speed of motor 3.
*/
{      
    double u1 = 2.0*(pot1 - 0.5);    // Normalised variable for the movement of motor 2
    if (u1>0)
    {   pin4 = true;
    }
    else if(u1<0)
    {   pin4 = false;    
    }
    pin5.period(fabs(u1));   // Set PWM period to 0.2 seconds
    //pin5 = fabs(u1);
    //float a1=fabs(u1);
    //pin5.write(a1);    // Set duty cycle (and thus the angle of the motor) to potmeter percentage
    
    double u2 = 2.0*(pot2 - 0.5);    // Normalised variable for the movement of motor 1
    if (u2<0)
    {   pin7 = true;
    }
    else if(u2>0)
    {   pin7 = false;    
    }
    pin6.period(fabs(u2));   // Set PWM period to 0.2 seconds
    //float a2 = fabs(u2);
    //pin6.write(a2);    // Set output duty cycle (and thus the angle of the motor) to potmeter percentage
}

// Main program
int main()
{   
    pc.baud(115200);
        
    motor.attach(turn, 0.001);
    //encoder.attach(Encoderinput,0.01);
        
    while   (true)
    {   // 
        Encoderinput();
    }
}