File content as of revision 0:c9d69c701daf:

// For the KL25Z
#include "mbed.h"

#define MOVE_FWD 'w'
#define MOVE_REV 's'
#define MOVE_LEF 'a'
#define MOVE_RIG 'd'
#define MOVE_STO 'q'

DigitalOut myled(LED2);
DigitalOut M1DIR(D7);
DigitalOut M2DIR(D8);
PwmOut M1PWM(D9);
PwmOut M2PWM(D10);
DigitalOut M_ON(D2);
Serial bt(D14, D15);
Serial pc(USBTX, USBRX);

bool move_motors(char c, int _power);
void motor_fwd(int _power);
void motor_rev(int _power);
void motor_left(int _power);
void motor_right(int _power);
void motor_stop();


int main() {
    myled = 0;
    wait(3);
    bt.baud(9600);
    M_ON = 1;
    while(1) {
        if (bt.readable()){
            char c = bt.getc();
            bt.printf("%c\n\r", c);
            move_motors(c, 100);
        }
    }
}

bool move_motors(char c, int _power){
    switch (c){
        case MOVE_FWD:
            motor_fwd(_power);
            break;
        case MOVE_REV:
            motor_rev(_power);
            break;
        case MOVE_LEF:
            motor_left(_power);
            break;
        case MOVE_RIG:
            motor_right(_power);
            break;
        case MOVE_STO:
            motor_stop();
            break;
        default:
            motor_stop();
            //bt.printf("%f\r\n", get_mag_angle());
    }
    return 1;
}

void motor_fwd(int _power){
    M1DIR = 1;
    M1PWM = 1;
    M2DIR = 1;
    M2PWM = 1;
    pc.printf("FWD\n");
}

void motor_rev(int _power){
    M1DIR = 0;
    M1PWM = 1;
    M2DIR = 0;
    M2PWM = 1;
    pc.printf("REV\n");
}

void motor_left(int _power){
    M1DIR = 0;
    M1PWM = 1;
    M2DIR = 1;
    M2PWM = 1;
    pc.printf("LEFT\n");  
}

void motor_right(int _power){
    M1DIR = 1;
    M1PWM = 1;
    M2DIR = 0;
    M2PWM = 1;
    pc.printf("RIGHT\n");
}

void motor_stop(){
    M1DIR = 1;
    M1PWM = 0;
    M2DIR = 1;
    M2PWM = 0;
    pc.printf("STOP\n");
}