File content as of revision 1:125af627e307:

#include "mbed.h"
#include "QEI.h"

#define SERIAL_BAUD 115200

//Initial allocations
Serial pc(USBTX,USBRX);

AnalogIn pot1(A1);
AnalogIn pot2(A2);

DigitalOut dirpin(D4);
DigitalOut dirpin2(D7);
PwmOut pwmpin(D5);
PwmOut pwmpin2(D6);
QEI encoder1(D12,D13,NC,64,QEI::X4_ENCODING);
QEI encoder2(D10,D11,NC,64,QEI::X4_ENCODING);

//parameters
double delta_t=0.01;
double alpha=360/(25*8400);
double L1=370/2;
double L2=65/2;
double pi=3.14159265359;
double beta=(((2*L1)-(2*L2))*20*pi)/(305*8400);

//functions        
double out1(){
    float a=(pot1*2.0f)-1.0f;
    return a;}
double out2(){
    double a=(pot2*2.0f)-1.0f;
    return a;}
int counts1(){
    int a=encoder1.getPulses();
    return a;}
int counts2(){
    int a=encoder2.getPulses();
    return a;}
double vdesx(){
    double a=out1()*20;
    return a;}
double vdesy(){
    double a=out2()*20;
    return a;}
double q1(){
    double a=counts1()*alpha;
    return a;}
double q2(){
    double a=counts2()*beta;
    return a;}
double MPe(){
    double a=L1-L2+q2();
    return a;}
double dq1(){    
    double a=((1/sin(q1()))*vdesx()-(1/cos(q1()))*vdesy())*0.5*delta_t*MPe();
    return a;}
double dq2(){  
    double a=((1/sin(q1()))*vdesx()+(1/cos(q1()))*vdesy())*0.5*delta_t*MPe();
    return a;}
int dC1(){
    int a= dq1()/alpha;
    return a;}
int dC2(){
    int a= dq2()/beta;
    return a;}
float pwm1(){
    float a=dC1()/delta_t;
    float b=a/8400;
    return b;}
float pwm2(){
    float a=dC2()/delta_t;
    float b=a/8400;
    return b;}
void start(){
    dirpin.write(pwm1() < 0);                    
    pwmpin = fabs (pwm1());                     
    dirpin2.write(pwm2() < 0);
    pwmpin2 = fabs (pwm2());}

//main 
int main(){
    pc.baud(115200);                               
    pc.printf("start\r\n");
    pwmpin.period_us(60);
    
    while(1){
        start();
        pc.printf("\r\n BOOM q2=%f \t dq2=%i \t dq1=%f \r\n MPe=%f \t vx=%f \t vy=%f \r\n counts1=%i \t counts2=%i \t out1=%f \t out2=%f \r\n",q2(),dq2(), dq1(), MPe(), vdesx(),vdesy(),counts1(),counts2(),out1(),out2());
        wait(delta_t);
        }
}