File content as of revision 9:466dff9ae128:

#include "mbed.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "math.h"
#define _USE_MATH_DEFINES
# define M_PI           3.14159265358979323846  /* pi */

DigitalOut ledr(LED_RED);
DigitalOut ledg(LED_GREEN);
DigitalOut ledb(LED_BLUE);

PwmOut  motor1_pwm(D5);
PwmOut  motor2_pwm(D6);
DigitalOut motor1_dir(D4);
DigitalOut motor2_dir(D7);
AnalogIn pot1(A0);
AnalogIn pot2(A1);

QEI motor1_enc(D12,D13,NC,8400,QEI::X4_ENCODING);
QEI motor2_enc(D10,D11,NC,8400,QEI::X4_ENCODING);  

InterruptIn  sw2(SW2);
InterruptIn  sw3(SW3);

MODSERIAL pc(USBTX, USBRX);

Ticker      sample_timer;

const float sample_time = 0.05; //s
const float sample_frequency = 200; //hz

double countstoangle(int counts){
    double angle;
    int counts_abs = abs(counts);
    if(counts_abs >= 8400){
       int partial_counts = counts_abs % 8400;
       angle = (2 * M_PI * partial_counts)/8400;
       return angle;
    }else{
        angle = (2 * M_PI * counts_abs)/8400;
        return angle;
    }
}

int main()
{
    ledr = 1;
    ledg = 1;
    ledb = 1;
    
    pc.printf("\r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n ~~~A$$De$troyer69~~~ \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n");
    
    motor1_pwm.period_us(60);
    motor1_pwm = 0;  
    motor2_pwm.period_us(60);
    motor2_pwm = 0;  
  
    while (true)
    {
            float pot1_float = pot1.read();
            float pot1_motor = (pot1_float * 2.0f) - 1.0f;
            int mot1_direction = pot1_motor >= 0;
            motor1_pwm.write(fabs(pot1_motor));
            motor1_dir = !mot1_direction;
            
            float pot2_float = pot2.read();
            float pot2_motor = (pot2_float * 2.0f) - 1.0f;
            int mot2_direction = pot2_motor >= 0;
            motor2_pwm.write(fabs(pot2_motor));
            motor2_dir = mot2_direction;
            
            double motor1_ang = countstoangle(motor1_enc.getPulses());
            double motor2_ang = countstoangle(motor2_enc.getPulses());
            
            pc.printf("Angle motor 1: %f pi rad, angle motor 2: %f pi rad.\r\n", motor1_ang/(M_PI), motor2_ang/(M_PI));
            
            wait(0.2f);
    }
}