File content as of revision 3:0edffb90e739:

#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);
    
    motor1.setPosition(200.0);
    motor2.setPosition(0);
    x=0;
    y=0;



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


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

        //x = x*10.0 + 69.8;
        //y = y*10.0 + 69.8;

        if(x < 30){
        theta = 0.25*pi;
        }
        if(y < 21){
        theta = 0.25*pi;
        }
        
        if(x >= 30 && y >= 21){
                theta   = atan(y/x)+(.25*pi);// *   (400.0/(.5*pi));
        }
        
        r       = (sqrt(x*x+y*y)) ;// *   (2577/461.335);
                

        theta_pen   = motor1.getPosition()  *   ((.5*pi)/400.0);
        r_pen       = motor2.getPosition()  *   (363.0/2196.0);
        
        
        dtheta  = (theta - theta_pen);
        dr      = (r - r_pen);

        theta_pwm   = (dtheta)*0.5;   
        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  |  r=%.3f   dr=%.3f  rpwm=%.3f\n", theta, dtheta, theta_pwm, r, dr, r_pwm);

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



    }
}