File content as of revision 16:720365110953:

// 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
*/
/* Isn't needed anymore --> directly used in QEI
DigitalIn pin8(D8);       // Encoder 1 B
DigitalIn pin9(D9);       // Encoder 1 A
InterruptIn pin10(D10);     // Encoder 2 B
InterruptIn pin11(D11);     // Encoder 2 A
InterruptIn pin12(D12);     // Encoder 3 B
InterruptIn 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

// Utilisation of libraries
MODSERIAL   pc(USBTX, USBRX);
Ticker      motor;
QEI         Encoder1(D8,D9,NC,32);      // Order of D8 and D9 still needs to be determined
QEI         Encoder2(D10,D11,NC,32);   
QEI         Encoder3(D12,D13,NC,32);
//Ticker      derivative;

// Global variables
//double    u1  = pot1;
double  u3 = 0.0;         // Normalised variable for the movement of motor 3

// Functions
void    turnbuttons()         
{   /*  Pressing button 2 concludes in a change of speed. While button 1 is pressed,
        the direction of change of speed is reversed. So pressing button 1 and 2
        simultaneously results for the turning speed of motor 3 in a slower movement,
        and eventually the motor will turn the other way around.
    */
    if  (button1 == 1 && button2 == 1)
        {   u3 = u3 + 0.1f;  //In stapjes van 0.1            
            if  (u3>1.0f)
            {   u3 = 1.0f;
            }
        }
    else if (button1 == 0 && button2 == 1)
        {   u3 = u3 - 0.1f;
            if  (u3>1.0f)
            {   u3 = 1.0f;
            }
        }
}

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.
*/
{   // Controls motor 1 velocity through potmeter 1   
    double  u1 = 2.0*(pot1 - 0.5);    // Normalised variable for the movement of motor 1
    if  (u1>0)
    {   pin4 = true;
    }
    else if (u1<0)
    {   pin4 = false;    
    }
    pin5 = fabs(u1);
    // Controls motor 2 velocity through potmeter 2
    double  u2 = 2.0*(pot2 - 0.5);    // Normalised variable for the movement of motor 2
    if  (u2<0)
    {   pin7 = true;
    }
    else if (u2>0)
    {   pin7 = false;    
    }
    pin6 = fabs(u2);
    // Controls motor 3 velocity through the buttons
    if  (u3>0)           //can maybe be deleted?
    {   pin2 = true;
    }
    else if (u3<0)
    {   pin2 = false; 
    }
    else
    {   pin3 = 0;
    }
    pin3 = fabs(u3);   
}
/*
//Derivative program to test, needs to work at 1000 Hz
void    derivefunction(double x, double xprev, double tsample)
{   double deriv = (x - xprev)/tsample;
    
}
*/

/* Emergency program to test
void    Emergency()
if emergencybutton == 1;
{   while (true)
    {
    }
}
*/

// Main program
int main()
{
    pc.baud(115200);
    
    pin5.period(1.0/10000);
    pin3.period_us(50);   
    pin5.period_us(50);    
    pin6.period_us(50);
    
    // Ticker
    motor.attach(turn, 0.001);
    //derivative.attach(derivefunction, 1/1000)
    
    // Interrupts
    button1.rise(&turnbuttons);       
    button2.rise(&turnbuttons);
    
    // Encoder, this code doesn't work
    // Our encoder doesn't have a NC, what do we need to fill in for that?
    int counts1 = Encoder1.getPulses();
    int counts2 = Encoder2.getPulses();
    int counts3 = Encoder3.getPulses();
        
    while   (true)
    {        
    }
}