File content as of revision 0:200635fa1b08:

// Eurobot13 main.cpp

#include "mbed.h"

#include "Actuators/MainMotor/MainMotor.h"
#include "Sensors/Encoder/Encoder.h"
#include "Actuators/Servo/Servo.h"

PwmOut Led(LED1);

void motortest();
void encodertest();
void motorencodetest();
void motorencodetestline();
void motorsandservostest();

int main() {
    //motortest();
    //encodertest();
    //motorencodetest();
    motorencodetestline();
    //motorsandservostest();
}

void motorsandservostest(){
    Encoder Eleft(p27, p28), Eright(p30, p29);
    MainMotor mleft(p24,p23), mright(p21,p22);
    Servo sTop(p25), sBottom(p26);
    Serial pc(USBTX, USBRX);
    const float speed = 0.0;
    const float dspeed = 0.0;
    
    Timer servoTimer;
    mleft(speed); mright(speed);
    servoTimer.start();
    while (true){
        pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
        if (Eleft.getPoint() < Eright.getPoint()){
            mleft(speed);
            mright(speed - dspeed);
        } else {
            mright(speed);
            mleft(speed - dspeed);
        }
        if (servoTimer.read() < 1){
            sTop.clockwise();
        } else if (servoTimer.read() < 4) {
            sTop.relax();
        } else if (servoTimer.read() < 5) {
            sBottom.anticlockwise();
            //Led=1;
        } else if (servoTimer.read() < 6) {
            sBottom.clockwise();
            //Led=0;
        } else if (servoTimer.read() < 7) {
            sBottom.relax();
        }else {
            sTop.anticlockwise();
        }
        if (servoTimer.read() >= 9) servoTimer.reset();
    }
}

void motorencodetestline(){
    Encoder Eleft(p27, p28), Eright(p30, p29);
    MainMotor mleft(p24,p23), mright(p21,p22);
    Serial pc(USBTX, USBRX);
    const float speed = 0.2;
    const float dspeed = 0.1;

    mleft(speed); mright(speed);
    while (true){
    //left 27 cm = 113 -> 0.239 cm/pulse
    //right 27 cm = 72 -> 0.375 cm/pulse
        pc.printf("Position is: %i \t %i \n\r", (int)(Eleft.getPoint()*0.239), (int)(Eright.getPoint()*0.375));
        if (Eleft.getPoint()*0.239 < Eright.getPoint()*0.375){
            mright(speed - dspeed);
        } else {
            mright(speed + dspeed);
        }
    }

}

void motorencodetest(){
    Encoder Eleft(p28, p27), Eright(p29, p30);
    MainMotor mleft(p23,p24), mright(p22,p21);
    Serial pc(USBTX, USBRX);
    
    const float speed = -0.3;
    const int enc = -38;
    while(true){
        mleft(speed); mright(0);
        while(Eleft.getPoint()>enc){
            pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
        }
        Eleft.reset(); Eright.reset();
        mleft(0); mright(speed);
        while(Eright.getPoint()>enc){
            pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
        }
        Eleft.reset(); Eright.reset();
    }
}

void encodertest(){
    Encoder E1(p28, p27);
    Encoder E2(p29, p30);
    Serial pc(USBTX, USBRX);
    while(1){
        wait(0.1);
        pc.printf("Position is: %i \t %i \n\r", E1.getPoint(), E2.getPoint());
    }

}
void motortest(){
    MainMotor mright(p22,p21), mleft(p23,p24);
    while(1) {
        wait(1);
        mleft(0.8); mright(0.8);
        wait(1);
        mleft(-0.2); mright(0.2);
        wait(1);
        mleft(0); mright(0);
    }
}