File content as of revision 6:93c9844af619:

#include "mbed.h"
#include "eCompass_Lib.h"
#include "MAG3110.h"
#include "MMA8451Q.h"
#include "rtos.h"
#include "SLCD.h"

#define MMA8451_I2C_ADDRESS (0x1d<<1)

#define espera  0.0001
#define sp      0.5     // set point 
#define sp0     0.5     // set point 
#define sp1     0.4     // set point 
#define idle    0
#define full    0.7     
#define k       100     // Divisor
#define lol     0.0
#define m       -1.333

PwmOut motor0(A2);    // PITCH arriba 
PwmOut motor1(A3);    // ROLL izq 
PwmOut motor2(A4);    // ROLL der 
PwmOut motor3(A5);    // PITCH abajo

/*PwmOut motor1(A2);    // PITCH arriba 
PwmOut motor0(A3);    // ROLL izq 
PwmOut motor3(A4);    // ROLL der 
PwmOut motor2(A5);    // PITCH abajo*/

DigitalIn   enable(SW1);
DigitalIn   btn2(SW3);
DigitalOut  led(LED1);
DigitalOut  led2(LED2);

int i=0,flag=0;
float xm=0, x=0;
float ym=0, y=0;

float pwmVal0=0, pwmVal1=0; // Valores auxiliares para imprimir
int i0=0,i1=0,i2=0,i3=0;
float scalerX=0.5;
float scalerY=0.5;
float difSample = 0, currSample = 0;
float c = 0;

eCompass compass;
MAG3110  mag( PTE25, PTE24);
MMA8451Q acc( PTE25, PTE24, MMA8451_I2C_ADDRESS);
DigitalOut red(LED_RED);
DigitalOut green(LED_GREEN);
Serial pc(USBTX, USBRX);
SLCD slcd;

extern axis6_t axis6;
extern uint32_t seconds;
extern uint32_t compass_type;
extern int32_t tcount;
extern uint8_t cdebug; 

MotionSensorDataCounts mag_raw;
MotionSensorDataCounts acc_raw;

// Declaracion de tareas
Thread *(debugp);
Thread *(calibrate);
Thread *(lcdp);
Thread *(bandera);
Thread *(controller);
Thread *(controllerY);

//
// Print data values for debug
//
void debug_print(void)
{    
    pc.printf("roll=%d, pitch=%d, yaw=%d\r\n", axis6.roll, axis6.pitch, axis6.yaw);
}

// HAL Map for KL46 Freedom board MMA8451Q & MAG3110 sensors
void hal_map( MotionSensorDataCounts * acc_data, MotionSensorDataCounts * mag_data)
{
    int16_t t;
    // swap and negate accelerometer x & y
    t = acc_data->y;
    acc_data->y = acc_data->x * -1;
    acc_data->x = t * -1;

    // negate magnetometer y
    mag_data->y *= -1;
    
}

//
// This is the 50Hz thread where the magic happens
//
int  l = 0;

void compass_thread(void const *argument) {
        // get raw data from the sensors
        mag.getAxis(mag_raw);
        acc.getAxis(acc_raw);
        if(tcount) compass.run( acc_raw, mag_raw); // calculate the eCompass
        if(!(l % 10)) lcdp->signal_set(0x04);
        if(l++ >= 10) { // take care of business once a second//50
            seconds++;
            bandera->signal_set(0x03);  
            controller->signal_set(0x05);
            controllerY->signal_set(0x06);
            calibrate->signal_set(0x2);
            //debugp->signal_set(0x01);
            l = 0;
            green = !green;
            }
        tcount++;
}

void calibrate_thread(void const *argument) {
    while(true) {
        Thread::signal_wait(0x2); 
        red = 0;
        compass.calibrate();
        red = 1;
        }
}
 
void print_thread(void const *argument) {
    while (true) {
        // Signal flags that are reported as event are automatically cleared.
        Thread::signal_wait(0x1);    
        debug_print();        
    }
}

void bandera_thread(void const *argument) {
    while (true) {
        // Signal flags that are reported as event are automatically cleared.
        Thread::signal_wait(0x3);  
        //pc.printf("xm=%f\r\n", xm);  
        if(!enable) 
        {
            flag=!flag;
            wait(0.5);
        } 
        else flag=flag;

        if(flag==1) 
        {
            led2=1;
            led=0;
        } 
        else 
        {    // Apaga los motores
            led2=0; // Green off
            led=1;  // Red on
            motor0 = lol;
            motor1 = lol;
            motor2 = lol;
            motor3 = lol;
        }
    }
}

// Funciones de Membresia para control por logica difusa
void controller_thread(void const *argument) {
    while (true) {
        // Signal flags that are reported as event are automatically cleared.
        Thread::signal_wait(0x5);  
        
        while (flag==1) 
        {     
            x = axis6.roll;
            xm=x/k;    
            
            int LE = 10;     // Low Error
            int ME = 10;    // Medium Error
            int HE = 30;    // High Error
            
            float m0 = -1.9;
            float m1 = -0.8;
            
            if (x > -ME || x < ME)
            {                    
                motor3 = sp-xm*m1*c;                
                wait(espera);                
                motor0 = sp+xm*m1*c;                
                wait(espera);  
                scalerX = 0.6;         
                pc.printf("vcxvcvcxvxcvxc");                               
            }
            if (x < -HE)
            {                
                motor3 = idle;
                wait(espera);                
                motor0 = full*scalerX*c;                
                wait(espera); 
                if (scalerX <=0.85)
                scalerX = scalerX+0.1;
                pc.printf("vcxvcvcxvxcvxc");
            }            
            if (x > HE)
            {                
                motor3 = full*scalerX*c;
                wait(espera);                
                motor0 = idle;
                wait(espera); 
                if (scalerX <=0.85)
                scalerX = scalerX+0.1;
                pc.printf("vcxvcvcxvxcvxc");
            }
            
            i2++;
            if (i2 >= 15)  currSample = xm;                
            if(i2 >= 30)
            {
                difSample = abs(xm -currSample);
                i2 = 0;
            }
            
            if (difSample >= 0.4)   c = 0.1;
            if (difSample < 0.4)   c = 1;                                        
                        
        }
    }
}

// Funciones de Membresia para control por logica difusa
void controllerY_thread(void const *argument) {
    while (true) {
        // Signal flags that are reported as event are automatically cleared.
        Thread::signal_wait(0x6);  
        
        while (flag==1) 
        {     
            y = axis6.pitch;
            ym=y/k;    
            
            int LE = 10;     // Low Error
            int ME = 17;    // Medium Error
            int HE = 30;    // High Error
            
            float my = -0.8;
            
            if (y > -ME && y < ME)
            {    
                motor2 = 0.2;//sp-ym*my;
                wait(espera);                                            
                motor1 = 0.2;//sp+ym*my;
                wait(espera);                
                scalerY = 0.6;                                        
            }
            if (y < -ME)
            {
                motor2 = idle; 
                wait(espera);            
                motor1 = full*scalerY;                
                wait(espera);                
                if (scalerY <=0.8)
                scalerY = scalerY+0.1;
            }            
            if (y > ME)
            {
                motor2 = full*scalerY; 
                wait(espera);            
                motor1 = idle;   
                wait(espera); 
                if (scalerY <=0.8)
                scalerY = scalerY+0.1;
            }
        }
    }
}

void lcd_thread(void const *argument) {
    while (true) {
        // Signal flags that are reported as event are automatically cleared.
        Thread::signal_wait(0x4);  
        slcd.printf("%04d", axis6.roll); // print the heading (NED compass) to the LCD             
    }
}
                                 
int main() 
{
    pc.baud (115200);
    slcd.clear();
    tcount = 0;
    red = 1;
    green = 1;
    //cdebug = 1; // Set to 1 to disable eCompass in order to observe raw mag data.
    compass_type = NED_COMPASS;
    seconds = 0;
    //Thread t1(print_thread);  
    Thread t2(calibrate_thread);
    //Thread t3(lcd_thread);        
    
    Thread t4(bandera_thread);
    Thread t5(controller_thread);
    Thread t6(controllerY_thread);
    
    //debugp = &t1;
    calibrate = &t2;
    //lcdp = &t3;  
    
    bandera = &t4;  
    controller = &t5;  
    controllerY = &t6;  
    
    mag.enable();
    acc.enable();

    mag.getAxis(mag_raw); // flush the magnetmeter
    RtosTimer compass_timer(compass_thread, osTimerPeriodic);
    
    /*
     * Thread Priorities
     * Compass Thread,      High Priority
     * Compass calibration, Above Normal
     * LED Update,          Normal
     * printf to console,   Below Normal
     * main(),              Normal
     */

    debugp->set_priority(osPriorityBelowNormal);
    lcdp->set_priority(osPriorityLow);
    calibrate->set_priority(osPriorityAboveNormal);    
    bandera->set_priority(osPriorityHigh);
    controller->set_priority(osPriorityAboveNormal);
    controllerY->set_priority(osPriorityAboveNormal);
    
    compass_timer.start(10); // Run the Compass every 20ms
    
    Thread::wait(osWaitForever);   
}