File content as of revision 32:da34d27a13f3:

//voltage in PWM (between -1 and 1)
//period in seconds
//dt the time between measurements, i.o.w. Ticker timing 

#include "mbed.h"
#include "FastPWM.h"
#include "QEI.h"
#include "global.h"

//Objects
    //Motors
        FastPWM motor1(D6); //Motor1 PWM output pin
        DigitalOut motor1Dir(D7); //Motor1 directional pin
        FastPWM motor2(D5); //Motor2 PWM output pin
        DigitalOut motor2Dir(D4); //Motor2 directional pin
    //Encoders
         QEI encoderMotor1(D12,D13,NC,64,QEI::X2_ENCODING);
         QEI encoderMotor2(D12,D13,NC,64,QEI::X2_ENCODING);

    //Variables
        static int countsMotor1[2]; //Array to store motor counts for i and i-1
        static int countsMotor2[2];
        static float velocityMotor1;
        static float velocityMotor2;
        static float angleMotor1;
        static float angleMotor2;
    //Constants
        const int uniqueMeasurementPoints = 4200; //From Canvas X4 = 8400, X1 = 2100, so X2 = 4200 https://canvas.utwente.nl/courses/4023/pages/project-materials?module_item_id=91422
        const double PI = 3.14159265358979;
        const float countsToRadians = (2*PI)/uniqueMeasurementPoints; //Number of radians per count
        
        

void motorAndEncoder(float PWM1, float PWM2, float dt)
{
    //Set motors
        //Direction
            if (PWM1<0)
            {
                PWM1 *= -1; //Shorthand for *-1
                motor1Dir.write(1); //negative direction
            }
            else
            {
                motor1Dir.write(0); //positive direction    
            }
            if (PWM2<0)
            {
                PWM2 *= -1; //Shorthand for *-1
                motor2Dir.write(1); //negative direction
            }
            else
            {
                motor2Dir.write(0); //positive direction    
            }
        //Period and PWM
            float periodPWM = 1/2000;
            motor1.period(periodPWM);
            motor1.write(PWM1);
        
            motor2.period(periodPWM);
            motor2.write(PWM2);
    //Read encoders
        //Counts
            //set the value from last loop to poisition 0 in the vector
                countsMotor1[0] = countsMotor1[1];
                countsMotor2[0] = countsMotor2[1];
            //set the new number of counts to position 1 in the vector
                countsMotor1[1] = encoderMotor1.getPulses();
                countsMotor2[1] = encoderMotor2.getPulses();
        //Angle
            angleMotor1 = countsMotor1[1]/countsToRadians;
            angleMotor2 = countsMotor2[1]/countsToRadians;        
        //Velocity calculation
            velocityMotor1 = ((countsMotor1[1]-countsMotor1[0])/countsToRadians)/dt; //rad/s
            velocityMotor2 = ((countsMotor2[1]-countsMotor2[0])/countsToRadians)/dt; //rad/s
    
    motorReturn.motor1.counts = countsMotor1[1];
    motorReturn.motor2.counts = countsMotor2[1];
    
    motorReturn.motor1.angle = angleMotor1;
    motorReturn.motor2.angle = angleMotor2;
    
    motorReturn.motor1.velocity = velocityMotor1;
    motorReturn.motor2.velocity = velocityMotor2;
}