File content as of revision 2:aae3b2c0881c:

#include "mbed.h"
#include "Servo.h"

#define MAXVEL      100  // From 100 max
#define SERINC      100   // Move micro seconds up/down


DigitalOut myled(LED1);
Serial pc(USBTX, USBRX);
Serial bt(PTE0, PTE1);
Servo SLF(D2);           // Servo Left Front
Servo SLB(D3);           // Servo Left Back
Servo SRF(D4);           // Servo Right Front
Servo SRB(D5);           // Servo Right Back
Servo SARM(D8);          // Servo Arm


void decodeInstruction(char instruction);
void setSpeed(int left, int right);
void moveServo(int increment);
void redLed(int status);

char c;


int main() {
    bt.baud(115200);
    while(1) {
        if (bt.readable()){
            c = bt.getc();
            pc.printf("Received: %c\n\r", c);
            decodeInstruction(c);
            printf("%c\n", c);
        }
            
    }
}


void decodeInstruction(char instruction){
    switch (instruction){
        case 'G':
        case 'g':
            setSpeed(MAXVEL, -MAXVEL);
            break;
        case 'A':
        case 'a':
            setSpeed(-MAXVEL, -MAXVEL);
            break;
        case 'S':
        case 's':
            setSpeed(-MAXVEL, MAXVEL);
            break;
        case 'D':
        case 'd':
            setSpeed(+MAXVEL, +MAXVEL);
            break;
        case 'Q':
        case 'q':
            setSpeed(0,0);
            break;
        case 'P':
        case 'p':
            moveServo(SERINC);
            break;
        case 'L':
        case 'l':
            moveServo(-SERINC);
            break;
        case 'W':
        case 'w':
            redLed(1);
            break;
        case 'H':
        case 'h':
            redLed(0);
            break;
    }        
}

void setSpeed(int left, int right){
    float sLeft=0, sRight=0;
    sLeft = (float)(left + 100)/200;
    sRight = (float)(right + 100)/200;
    SLF = sLeft;
    SLB = sLeft;
    SRF = sRight;
    SRB = sRight;
}

void moveServo(int increment){
    float temp = 0.0;
    temp = (float)increment / 100.0;
    if (temp < 0 && SARM >= abs(temp)){
        SARM = SARM + temp;
    }else if (temp > 0 && SARM <= 1 - abs(temp)){
        SARM = SARM + temp;
    }
}

void redLed(int status){
    myled = status;
}