File content as of revision 0:ed3e71232bc7:

#include "mbed.h"
#include "XNucleo53L0A1.h"
#include "L3G4200D_my.h"
#include "sMotor.h" 
#include "lis3mdl_class.h"

/****************************************************************
 *                        Definitions                           *                                              
*****************************************************************/

/*****************************************************************
 *                         Prototypes                            *       
 *****************************************************************/
void button1_enabled_cb(void);
void button1_onpressed_cb(void);
void Ini();
void proximityR_isr();
void proximityF_isr();
void polling_sensors_isr();
void sensors_task();
void execute_pc(int events);
void DurationMesure(uint32_t *dt);



/*****************************************************************
*                     Interface                                  *               
******************************************************************/
DigitalOut led1(LED1);
InterruptIn button1(USER_BUTTON);
RawSerial pc(USBTX, USBRX);
DevI2C *device_i2c;
static XNucleo53L0A1 *board=NULL;
sMotor motor(A5, A4, A3, A1);
InterruptIn proximity(A0);
DevI2C devI2c(D14,D15);
LIS3MDL magnetometer(&devI2c,LIS3MDL_M_MEMS_ADDRESS_LOW);



/*****************************************************************
*                         Threads                                *
******************************************************************/
Thread sensor_daemon;


/*****************************************************************  
*                     Time                                       *
******************************************************************/
Timeout button1_timeout; // Used for debouncing
Ticker polling_sensors;
Timer DurationTimer; 


/*****************************************************************
*                     Global variables                           *              
******************************************************************/
volatile bool button1_pressed = false; // Used in the main loop
volatile bool button1_enabled = true; // Used for debouncing
uint8_t mode=0;
uint8_t prox=0;
uint8_t SensorsEn=1;
uint32_t distance_c;
uint32_t distance_l;
uint32_t distance_r;
int16_t G[3];
char rxpc_buffer[128];
uint32_t TaskDurationL; 
uint32_t TaskDurationR;
uint32_t TaskDurationC;
uint32_t TaskDurationG;
uint8_t direction;
uint8_t point = 0;


/***************************************************************** 
*                      Main                                      *
******************************************************************/
int main()
{
    Ini();
    while (true) {
        if(button1_pressed){
            mode++;
            if(mode>4)
                mode=0;
            button1_pressed=false;  
        }
        if(point){
           motor.step(point, direction, 5000); 
        }else {
           wait(0.05);    
        } 
    }
}

/***********************************************************
*            Functions                                     *
***********************************************************/
void Ini()
{ 
//    thread.start(print_thread);
    //button1.mode(PullUp); // Activate pull-up
    // Attach ISR to handle button press event  
    button1.fall(callback(button1_onpressed_cb)); 
    pc.baud(115200);
    
    device_i2c = new DevI2C(D14, D15);
    board = XNucleo53L0A1::instance(device_i2c, A2, D8, D2);
    int status = board->init_board();
    if (status) 
        pc.printf("Failed to init XNucleo53L0A1 board!\r\n");
    char text[5];
    sprintf(text,"mbed");  
    board->display->display_string(text);  
    GyroL3G4200D_Ini(device_i2c);  
    proximity.mode(PullUp);  
    proximity.rise(&proximityR_isr); 
    proximity.fall(&proximityF_isr);
    mode=5;
    wait(2.0);
    sensor_daemon.start(sensors_task); 
    SensorsEn=1;
    polling_sensors.attach(&polling_sensors_isr, 0.4);
    pc.read((uint8_t *)rxpc_buffer,
    128, &execute_pc, SERIAL_EVENT_RX_ALL,10);
    




 }

//-----------------------
void DurationMesure(uint32_t *dt)
{
  DurationTimer.stop(); 
 *dt=DurationTimer.read_us(); 
   DurationTimer.reset();       
}  





//-------------------------------------------
void sensors_task()
{
 int status;
 char text[5]; 
 
    while (true) {
        ThisThread::flags_wait_any(0x1,true);
        SensorsEn=0;
        //DurationTimer.start();                 
        status = board->sensor_left->get_distance(&distance_l); 
        if (status != VL53L0X_ERROR_NONE)
            distance_l=8888;  
        //DurationMesure(&TaskDurationL);
        //DurationTimer.start();   
        status = board->sensor_right->get_distance(&distance_r);
        if (status != VL53L0X_ERROR_NONE)
             distance_r=8888;      
        //DurationMesure(&TaskDurationR);
        //DurationTimer.start();   
        status = board->sensor_centre->get_distance(&distance_c); 
        if (status != VL53L0X_ERROR_NONE)
             distance_c=8888; 
       //DurationMesure(&TaskDurationC);
       
        switch(mode){
            case 0:
                point = 0;
                sprintf(text,"c%ld",distance_c);  
                break;
            case 1:
                point = 0;
                sprintf(text,"l%ld", distance_l);                        
                break;  
            case 2:      
                point = 0;
                sprintf(text,"r%ld", distance_r);
                break;    
            case 3:      
                int a = abs((int)distance_r-(int)distance_l);
                sprintf(text,"d%ld",a);
                
                if(a<10){
                    point = 0;
                }else{
                    if ((int)distance_r>(int)distance_l)
                    {
                        point = 1;
                        direction = 1;
                    }
                    else 
                    {
                        point = 1;
                        direction = 0;
                    }
                }
                                       
                break;   
            case 4:      
            
                sprintf(text,"%d",G[0]);
                direction = 0;
                point = 1;    
                if(prox)
                    point = 0;
                break;             
            default: 
                sprintf(text,"End");                    
                break;                      
        } 
        board->display->display_string(text);                                 
        SensorsEn=1;
   }         
}
//------------------------------
void execute_pc(int events)
{
 char *endptr;
  
 if(SERIAL_EVENT_RX_CHARACTER_MATCH & events)   
switch(rxpc_buffer[0]) {
      case 'T':
      case 't':
          pc.printf("DL=%ld,DR=%ld,DC=%ld,DG=%ld",
          TaskDurationL,TaskDurationR,TaskDurationC,TaskDurationG);         
          break;   
      case 'S':
      case 's':
            pc.printf("DC=%d, DL=%d, DR=%d, PROX=%d, GYRO=%d,%d,%d",
         distance_c,distance_l,distance_r,proximity?1:0,G[0],G[1],G[2]);
            break;    
      case 'M':
      case 'm':        
          mode=strtol(&rxpc_buffer[1],&endptr,10);
          break;                                     
   } 
else
 pc.printf("evtnt=%d", events); 
pc.read((uint8_t *)rxpc_buffer, 128,
   &execute_pc,SERIAL_EVENT_RX_ALL,10);//(unsigned char)'\n');  
}  


/***********************************************************
*         Interrupt Service Routines                       *
***********************************************************/
//----------------------------ISR handling button pressed event
void button1_onpressed_cb(void)
{
    if (button1_enabled) {// Disabled while the button is bouncing
        button1_enabled = false;
        button1_pressed = true; // To be read by the main loop
         // Debounce time 50 ms
button1_timeout.attach(callback(button1_enabled_cb), 0.03); 
    }
}

//---------------------------Enables button when bouncing is over
void button1_enabled_cb(void)
{
    button1_enabled = true;
}
//--------------------------------
void proximityR_isr()
{
  prox=1;      
} 
//--------------
void proximityF_isr()
{ 
  prox=0;      
}    
//------------
 void polling_sensors_isr()
{
 if (SensorsEn){
    sensor_daemon.flags_set(0x1);
    led1 = !led1; 
 }    
}