File content as of revision 2:664717ad13cc:

// Micromouse 2
// Further Test out the motors.
// Control with PWM.
// Test Serial Output.

////////////////
//SOME GLOBALS//
////////////////
int turn_dist = 1000;


#include "mbed.h"
#include "PinDefinitions.h"
Serial pc(PC_6, PC_7);        // These pins work for serial communications.
InterruptIn mybutton(USER_BUTTON);
DigitalOut led3(LED3);
DigitalOut motor_enabler(PB_6);
 
int Rdistance() { return RENC.getPulses(); }
int Ldistance() { return -LENC.getPulses(); }
 
 
void setLeftPwm(float speed) {
    if (speed == 0) {
        LMOTORA = 1.0;
        LMOTORB = 1.0;
    }
    
    if (speed > 0) {
        LMOTORA = speed;
        LMOTORB = 0;
    }
    else {
        LMOTORA = 0;
        LMOTORB = -speed;
    }
}
 
void setRightPwm(float speed) {
    if (speed == 0) {
        RMOTORA = 1.0;
        RMOTORB = 1.0;
    }
    
    if (speed > 0) {
        RMOTORA = speed;
        RMOTORB = 0;
    }
    else {
        RMOTORA = 0;
        RMOTORB = -speed;
    }
}
 
void pressed()
{
    led3 = !led3;
    pc.printf("Hellooooo\r\n");
}
 

void goForward(int dist)
{
    setRightPwm(1);
    setLeftPwm(0.95);
    while (Rdistance() < dist && Ldistance() < dist)
    {
        pc.printf("Lp: %d\nRp: %d\n\n", Ldistance(), Rdistance());
    }
    setRightPwm(0);
    setLeftPwm(0);
    RENC.reset();
    LENC.reset();
}

void turnR()
{   
    setLeftPwm(1);
    while(Ldistance() < turn_dist)
    {
        pc.printf("Turning... Lp: %d\n", Ldistance());
    }
    setLeftPwm(0);
    RENC.reset();
    LENC.reset();
}

void turnL()
{    
    setRightPwm(1);
    while(Rdistance() < turn_dist)
    {
        pc.printf("Turning... Rp: %d\n", Rdistance());
    }
    setRightPwm(0);
    RENC.reset();
    LENC.reset();
}

int main() {
    mybutton.fall(&pressed);
    led3 = 0;
    motor_enabler = 1;
    
    goForward(1400);
    //wait(1);
    turnR();
    //wait(1);
    goForward(700);
    turnL();
    goForward(1200);

    pc.printf("Done.\n");
}