File content as of revision 0:ab931824163f:

/* ==================================================================================
    --- TEST 1 ---
    OBJETIVE:
    Move the motor via bluetooth and program 90° turns to the right and left using 
    the magnometer.
    
    COMPONENTS:
    * Bluetooth
    * Magnometer
    * Motors
    
    CONTROLS:
    w: foward
    s: reverse
    a: left turn
    d: right turn
    o: 90° left turn
    p: 90° right turn

================================================================================== */

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

#define MOVE_FWD 'w'
#define MOVE_REV 's'
#define MOVE_LEF 'a'
#define MOVE_RIG 'd'
#define MOVE_STO 'q'
#define MOVE_90L 'o'
#define MOVE_90R 'p'

// ----- Objects --------------------------------------------------------------------
Serial pc(USBTX, USBRX); // Primary output to demonstrate library
Serial bt(PTC15, PTC14);
// Pin names for FRDM-K64F
FXOS8700CQ fxos(PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1); // SDA, SCL, (addr << 1)
InterruptIn fxos_int2(PTC13); // should just be the Data-Ready interrupt
DigitalOut led_on(LED2);
DigitalOut tled(LED3);
// MOTORS
DigitalOut in1_A(D2);
DigitalOut in2_A(D4);
PwmOut en_A(D3);
DigitalOut in1_B(D6);
DigitalOut in2_B(D7);
PwmOut en_B(D5);

// ----- Others ---------------------------------------------------------------------
SRAWDATA accel_data;
SRAWDATA magn_data;

// ----- Variables ------------------------------------------------------------------
double mag_x, mag_y;
//double mag_angle, mag_old_angle;

// ----- Function prototypes --------------------------------------------------------
void prints();
void trigger_fxos_int2();
double get_mag_x();
double get_mag_y();
double get_mag_angle();
void move_motors(char _move_command, int _power);
void motor_fwd(int _power);
void motor_rev(int _power);
void move_90(int direction);
void motor_left(int _power);
void motor_right(int _power);
void motor_stop();


// ----- Main program ---------------------------------------------------------------
int main(){
    bt.baud(9600); 
    fxos_int2.fall(&trigger_fxos_int2);
    fxos.enable();
    prints();
    
    while (true){
        
        //mag_angle = get_mag_angle();
        
        if (bt.readable()){
            char c = bt.getc();
            bt.printf("%c\n\r", c);
            move_motors(c, 100);
        }
        
        
        //pc.printf("Angle: %f\r\n", mag_angle);
        //wait(1);
    }  
} 

// ----- Functions ------------------------------------------------------------------
void prints(){
    bt.printf("\r\n\nFXOS8700Q Who Am I= %X\r\n", fxos.get_whoami());
    bt.printf("\r\nAccelerometer scale %X\r\n", fxos.get_accel_scale());
    bt.printf("\r\nInit mag angle %3.2f\r\n", get_mag_angle());
    bt.printf("----------------------------------\r\n\r\n");
    wait(1);
}

void trigger_fxos_int2(){
    //fxos_int2_triggered = true;
    //us_ellapsed = t.read_us();
}

double get_mag_x(){
    fxos.get_data(&accel_data, &magn_data);
    return magn_data.x;
}

double get_mag_y(){
    fxos.get_data(&accel_data, &magn_data);
    return magn_data.y;
}

double get_mag_angle(){
    double _mag_angle;
    
    mag_x = get_mag_x();
    mag_y = get_mag_y();
    
    _mag_angle = atan2(mag_x, mag_y)*180/3.14159;
    
    if (_mag_angle < 0){
        _mag_angle = 360 + _mag_angle;
    }
    
    return _mag_angle;
}

void move_motors(char _move_command, int _power){
    switch (_move_command){
        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;
        case MOVE_90L:
            move_90(1);
            break;
        case MOVE_90R:
            move_90(2);
            break;
        default:
            bt.printf("%f\r\n", get_mag_angle());
    }
}

void move_90(int direction){
    double old_angle = get_mag_angle();
    double tarjet = 0;
    
    bt.printf("giro de 90");
    
    if (direction == 1){
        tarjet = old_angle - 90;
        if (tarjet < 90){
            tarjet = 360 + tarjet;
        }
    }else if (direction == 2){
        tarjet = old_angle + 90;
        if (tarjet >= 360){
            tarjet = tarjet - 360;
        }
    }
    
    bt.printf("Tarjet: %f\r\n", tarjet);
    
    int motor_speed = 75; // Velocidad inicial
    double actual_angle;
    while (true){
        actual_angle = get_mag_angle();
        bt.printf("actual angle: %f \r\n", actual_angle);
        if (direction == 1){
            // Realizar un PI o P para calcular la velocidad
            motor_left(motor_speed);
            if (actual_angle > tarjet - 0.05 &&  actual_angle < tarjet + 0.05){
                // En mente tomar 50 muestras lo mas rapido posible
                // y cuando salgan que en promedio las lecturas tienen
                // un error menor o igual a +/- 0.5% entonces salirse
                break;
            }
        }else{
            motor_right(motor_speed);
            if (actual_angle > tarjet - 0.05 &&  actual_angle < tarjet + 0.05){
                break;
            }
        }
    }       
}

void motor_fwd(int _power){
    in1_A = 1;
    in2_A = 0;
    en_A = (float)_power/100;
    in1_B = 1;
    in2_B = 0;
    en_B = (float)_power/100;
    pc.printf("FWD\n");
}

void motor_rev(int _power){
    in1_A = 0;
    in2_A = 1;
    en_A = (float)100/_power;
    in1_B = 0;
    in2_B = 1;
    en_B = (float)_power/100;
    pc.printf("REV\n");
}

void motor_left(int _power){
    in1_A = 1;
    in2_A = 0;
    en_A = (float)_power/100;
    in1_B = 0;
    in2_B = 1;
    en_B = (float)_power/100;
    pc.printf("LEFT\n");
}

void motor_right(int _power){
    in1_A = 0;
    in2_A = 1;
    en_A = (float)_power/100;
    in1_B = 1;
    in2_B = 0;
    en_B = (float)_power/100;
    pc.printf("RIGHT\n");
}

void motor_stop(){
    in1_A = 0;
    in2_A = 0;
    en_A = 0;
    in1_B = 0;
    in2_B = 0;
    en_B = 0;
    pc.printf("STOP\n");
}