File content as of revision 0:29436844c427:

/*
MAPA DE CANAIS
Bluetooth_TX    E1
Bluetooth_RX    E0
Motor_Amarelo   B0
Motor_Laranja   B1
Motor_Verde     B2
Motor_Azul      B3
Servo_R         D4
Servo_L         A12


*/

// D4 = D1 
// D5 = D3
// D6 = D2
// D7 = D0
// RS = D5
// E  = A13

#include "mbed.h"
#include "MMA8451Q.h"
#include "TextLCD.h"

#define MMA8451_I2C_ADDRESS (0x1d<<1)

#define SERVO_L_MAX 800
#define SERVO_L_MIN 0
#define SERVO_R_MAX 800
#define SERVO_R_MIN 0
#define MIN_PULSO_SERVO_US 700
 



Serial bluetooth(PTE0, PTE1);
Serial pc(USBTX, USBRX);

TextLCD LCD(PTD5, PTA13,PTD1, PTD3, PTD2, PTD0);
MMA8451Q acelerometro(PTE25, PTE24, MMA8451_I2C_ADDRESS);

PwmOut motor_L2(PTD4);
PwmOut motor_L1(PTA12);
PwmOut motor_R1(PTA4);
PwmOut motor_R2(PTA5);

PwmOut Servo_L(PTD5);
PwmOut Servo_R(PTA13);

DigitalOut Led_Vermelho(LED_RED);
DigitalOut Led_Verde(LED_GREEN);
DigitalOut Led_Azul(LED_BLUE);

long count = 0;

float velocidade = 1;

float d_x;
float d_y;
float d_z;
float old_x = 0;
float old_y = 0;
float old_z = 0;


void nome_integrantes(){
    LCD.printf("nome do robo");
    wait(1);
    LCD.cls();
    LCD.printf("Adriana Peter");
    wait(1);
    LCD.cls();
    LCD.printf("Julia Dias");
    wait(1);
    LCD.cls();
    LCD.printf("Marcos Torres");
    wait(1);
    LCD.cls();
    LCD.printf("Thiago Vinicius");
    wait(1);
    LCD.cls();
    
}

void abre_arma(){
    Servo_L.pulsewidth_us(MIN_PULSO_SERVO_US + SERVO_L_MAX);
    Servo_R.pulsewidth_us(MIN_PULSO_SERVO_US + SERVO_R_MIN);
}

void fecha_arma(){
    Servo_L.pulsewidth_us(MIN_PULSO_SERVO_US + SERVO_L_MIN);
    Servo_R.pulsewidth_us(MIN_PULSO_SERVO_US + SERVO_R_MAX);
}

char b(){
    char cmd;   
    bluetooth.scanf("%c", &cmd);
           
    return cmd;  
}

void movimentos(){
        char comando;
        comando = b();
        switch (comando){
            
            case 'F':  //frente
                motor_L1 = 0;
                motor_L2 = velocidade;
                motor_R1 = 0;
                motor_R2 = velocidade;
            break;
            
            case 'B': //ré
                motor_L2 = 0;
                motor_L1 = velocidade;
                motor_R2 = 0;
                motor_R1 = velocidade;
            break;
            
            case 'R':
                motor_L2 = 0;
                motor_L1 = velocidade;
                motor_R1 = 0;
                motor_R2 = velocidade;
            break;
            
            case 'L':
                motor_L1 = 0;
                motor_L2 = velocidade;
                motor_R2 = 0;
                motor_R1 = velocidade;
            break;
            
            case 'I': //frente esquerda
                motor_L1 = 0;
                motor_L2 = 0;
                motor_R1 = 0;
                motor_R2 = velocidade;
            break;
            
            case 'G': //frente direita
                motor_L1 = 0;
                motor_L2 = velocidade;
                motor_R1 = 0;
                motor_R2 = 0;
            
            break;
            
            case 'J': //re esquerda
                motor_L1 = 0;
                motor_L2 = 0;
                motor_R2 = 0;
                motor_R1 = velocidade;
            break;
            
            case 'H': // re direita
                motor_L2 = 0;
                motor_L1 = velocidade;
                motor_R1 = 0;
                motor_R2 = 0;
            break;
            
            case 'S': // parado
                motor_L1 = 0;
                motor_L2 = 0;
                motor_R1 = 0;
                motor_R2 = 0;
            break;
            
            case 'w':
                fecha_arma();
            break;
            
            case 'W':
                abre_arma();
            break;
         
            default:
            if(comando == '0') velocidade = 0;
            else if (comando == '1') velocidade = 0.1;
            else if (comando == '2') velocidade = 0.2; 
            else if (comando == '3') velocidade = 0.3;
            else if (comando == '4') velocidade = 0.4;
            else if (comando == '5') velocidade = 0.5;
            else if (comando == '6') velocidade = 0.6;
            else if (comando == '7') velocidade = 0.7;
            else if (comando == '8') velocidade = 0.8;
            else if (comando == '9') velocidade = 0.9;
            else if (comando == 'q') velocidade = 1;
            
            
            
    }
    pc.printf("%c || %i\n", comando, count++);
}


void seta(){
    d_x = abs(acelerometro.getAccX() - old_x);
    d_y = abs(acelerometro.getAccY() - old_y);
    d_z = abs(acelerometro.getAccZ() - old_z);
        bluetooth.printf("%f | %f | %f\n", d_x, d_y, d_z);
}
int main() {
    
    Servo_L.period_ms(20);
    Servo_R.period_ms(20);
    abre_arma();
    Led_Vermelho = 1;
    Led_Verde = 1;
    Led_Azul = 1;
    while(1) {
        movimentos();
        seta();
    }

}