File content as of revision 0:f6ff6d8ecfc0:

/*
    OSER - Open Source Educational Robot

    Author: 
        Gerardo Carmona
    Webpage: 
        www.makerobots.tk

    About:
        Open source and educational robot aimed to promote science and technology
        over students between 15 to 17 years old.

    Licence:
        I provide this information so students, teachers, and all people interested 
        on build their own robot can get started with an easy to use robot. All 
        information is provided as Open Source with MIT License 2016, more details 
        visit https://opensource.org/licenses/MIT.

*/


// --- Libraries ------------------------------------------------------------------
#include "mbed.h"
#include "Servo.h"

// --- Definitions ----------------------------------------------------------------
#define MAXVEL      100   // 100 is max
#define SERINC      100   // Move micro seconds up/down (range is 1000 to 2000)

// --- I/O pins -------------------------------------------------------------------
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 S01(D8);           // Servo Arm


// --- Prototypes -----------------------------------------------------------------
void decodeInstruction(char instruction);
void setSpeed(int left, int right);
void moveServo(int increment);

// --- Variables ------------------------------------------------------------------
char c;

// --- Main program ---------------------------------------------------------------
int main() {
    // Bluetooth baud rate
    bt.baud(115200);
    while(1) {
        // Search for data and decode instruction
        if (bt.readable()){
            c = bt.getc();
            decodeInstruction(c);
        }
            
    }
}

// --- Functions ------------------------------------------------------------------
void decodeInstruction(char instruction){
    switch (instruction){
        case 'W':
        case 'w':
            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;
    }        
}

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;
    float pos;
    temp = (float)increment / 100.0;
    pos = S01;
    if (temp < 0 && pos >= abs(temp)){
        S01 = pos + temp;
    }else if (temp > 0 && pos <= 1 - abs(temp)){
        S01 = pos + temp;
    }
}