File content as of revision 0:155294201f36:

#include "mbed.h"
#include "encoder.h"
#include "MODSERIAL.h"

// Maken van een looptimer.
volatile bool looptimerflag;
void setlooptimerflag(void)
{
    looptimerflag = true;
}

int main()
{

// Communicatie voor pc
    MODSERIAL pc(USBTX,USBRX);
    pc.baud(115200);

// Voor aansturen motoren
    double pi;
    pi=3.14159265359;

    double x,y;



// Variabelen benoemen voor regelaar motor.
    double theta, theta_pen, up_theta, kp_theta, kd_theta, dtheta, ei_theta, ui_theta, ki_theta, ed_theta, u_theta, ud_theta, theta_pwm;
    double r, r_pen, up_r, kp_r, kd_r, dr, ei_r, ui_r, ki_r, ed_r, u_r, ud_r, r_pwm;
    double motor1_maxu, motor2_maxu;
    
    double Ts;
    
// Sample tijd
    Ts = 0.001;

// Pinnen voor potmeter
    AnalogIn potmeter1(PTB2);
    AnalogIn potmeter2(PTB3);

// Pinnen voor encoder
    /* First pin should be PTDx or PTAx because those pins can be used as interruptIn */
    Encoder motor1(PTD0,PTC9);
    Encoder motor2(PTD2,PTC8);

    /* PWM naar motor */
    PwmOut pwm_motor1(PTA12);
    PwmOut pwm_motor2(PTA5);

    /* Pin voor richting */
    DigitalOut motor1dir(PTD3);
    DigitalOut motor2dir(PTD1);

    //Tijd looptimer instellen.
    Ticker looptimer;
    looptimer.attach(setlooptimerflag,Ts);


// Oneidige loop...
    while(true) {
        while(looptimerflag != true);
        looptimerflag = false;

        x = (potmeter1.read()*297.0+69.8);
        y = (potmeter2.read()*210.0+69.8);


        theta   = atan(y/x)       ;// *   (400.0/(.5*pi));
        r       = (sqrt(x*x+y*y)) ;// *   (2577/461.335);
        

        theta_pen   = motor1.getPosition()  *   ((.5*pi)/400);
        r_pen       = motor2.getPosition()  *   (363/2577);
        
        
        dtheta  = (theta - theta_pen);
        dr      = (r - r_pen);

        dtheta  =  

        theta_pwm   = (dtheta)*0.001;   
        r_pwm       = (dr)*0.001;

        //NAAR MOTOR
        
        //Zorgen dat pwm tussen -1 en 1 blijft.
        if(theta_pwm > 1) {
            theta_pwm=1;
        }
        if(theta_pwm < -1) {
            theta_pwm=-1;
        }
        if(r_pwm > 1) {
            r_pwm=1;
        }
        if(r_pwm < -1) {
            r_pwm=-1;
        }

        // Bepaal richting waarin motoren moeten draaien
        if(theta_pwm > 0)
            motor1dir.write(1);
        else
            motor1dir.write(0);
        if(r_pwm > 0)
            motor2dir.write(1);
        else
            motor2dir.write(0);
        
        // print naar pc
        pc.printf("t=%.3f   dt=%.3f  tpwm=%.3f\n", theta, dtheta, theta_pwm);

        //schrijf PWM naar motor
        pwm_motor1.write(abs(theta_pwm));
        pwm_motor2.write(abs(r_pwm));



    }
}