File content as of revision 6:d0f5da9962f5:

// main
#include "mbed.h"
#include <stdio.h>
#include <math.h>
#include "QEI.h"
#include "Point.cpp"

Serial pc(USBTX, USBRX);
AnalogIn pot1(A0); // sets requested Vx
AnalogIn pot2(A1); // sets requested Vy
QEI encMotor1 (D12, D13, NC, 624);
QEI encMotor2 (D10, D11, NC, 624);
float round=4200;
float angle1;
float angle2;
Point pCurrent;
Point pTo;
float toX;
float toY;
float deltaAngle1;
float deltaAngle2;
float v1;
float v2;
float timestep=1;
int main()
{
    while(true){
        // first get the current position from the motor encoders
        angle1=encMotor1.getPulses()/round*360;
        angle2=encMotor2.getPulses()/round*360;
        pCurrent.fromRotational(angle1,angle2);
        
        
        // calculate the position to go to according the the current position + the distance that should be covered in this timestep
        toX=pCurrent.posX+pot1.read()*timestep;
        toY=pCurrent.posY+pot2.read()*timestep;
        
        pTo.fromCarthesian(toX, toY);
        
        // calculate how much the angles should change in this timestep
        deltaAngle1=pTo.rotA-pCurrent.rotA;
        deltaAngle2=pTo.rotB-pCurrent.rotB;
        
        // calculate the neccesairy velocities to make these angles happen.
        v1=deltaAngle1/timestep;
        v2=deltaAngle2/timestep;
        
        // output to the user
        pc.printf("now: angle1: %f, angle2: %f, x: %f, y: %f \n\r",pCurrent.rotA,pCurrent.rotB,pCurrent.posX,pCurrent.posY);
        pc.printf("to:  angle1: %f, angle2: %f, x: %f, y: %f \n\r",pTo.rotA,pTo.rotB,pTo.posX,pTo.posY);
        pc.printf("v1: %f, v2: %f \n\r",v1,v2);
        wait(timestep);
    }
}