File content as of revision 0:eb99909151eb:

#include "mbed.h"
#include "platform/mbed_thread.h"
#include "UserButton.h"
#include "sMotor.h" 
#include "lis3mdl_class.h"
#include "QuadDisplay2.h"

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

// Blinking rate in milliseconds
#define step_speed 1500
#define POINTS 1
#define M_PI 3.14159265358979323846
#define RADIAN M_PI/180.0

/****************************************************************
*                          Prototypes   
*****************************************************************/

void Ini();
void HowManyClicks();
void proximityR_isr();
void proximityF_isr();
//-----------------------
void polling_sensors_isr();
void sensors_task();
//-----------------------
void clk_speed_isr();
/*****************************************************************
*                           Interface
******************************************************************/

DigitalOut led1(LED1);
DigitalIn CW_switch(D10);
DigitalIn CCW_switch(D11);
//---
RawSerial pc(USBTX, USBRX);
DevI2C devI2c(D14,D15);
LIS3MDL magnetometer(&devI2c,LIS3MDL_M_MEMS_ADDRESS_LOW);
DigitalOut led2(A0);
SPI spi(D4, D5, D3);
QuadDisplayMy display(&spi,D6);
//---
sMotor motor(A5, A4, A3, A2); // creates new stepper motor: IN1, IN2, IN3, IN4
InterruptIn BlueBatton(BUTTON)

/*****************************************************************
 *                            Time
******************************************************************/
//Ticker polling_sensors;
//Ticker clk_speed;
/****************************************************************
*                           Threads  
****************************************************************/
//Thread sensor_daemon;

/*****************************************************************
*                             Global variables
******************************************************************/


//----------------
uint8_t button_event;
uint8_t mode=0;
uint8_t RotateDir;
uint16_t RotateSpeed;
//----------------
uint8_t SensorsEn=1;

int16_t M[3];
uint8_t point;
uint32_t rotate_cnt;
//---------
//------------------

/******************************************************************
 *                              Main 
******************************************************************/
int main()
{
    // Initialise the digital pin LED1 as an output
    
    Ini();
    
    while (true) {
        //led = !led;
        HowManyClicks();
        if (mode){
            
        }
        if (point){
            motor.step(POINTS, RotateDir, RotateSpeed);
        }else
           wait_us(step_speed);
             
        //thread_sleep_for(BLINKING_RATE_MS);
    }
}

/**********************************************************
*                              Functions  
***********************************************************/
void Ini()
{
 pc.baud(115200);
 ButtonIni(&BlueBatton);
 button_event_ind=button_event=0;
 
 
//--------------

sensor_daemon.start(sensors_task);
SensorsEn=1;
polling_sensors.attach(&polling_sensors_isr, 0.4);
clk_speed.attach(&clk_speed_isr, 0.01);
mode=0;
point=0;
RotateSpeed=step_speed;
rotate_cnt=0;
}

//------------------
void HowManyClicks()
{
    WhatButtonMode(&button_event)
    if(button_event) {
        switch (button_event) {
            case 1:
                mode=0;
                point=0;
                break;
            case 2:
                mode=1;
                point=0;
                //RotateDir=1;
                //RotateSpeed=step_speed;
                break;                    
            case 3:
                mode=2;
                point=0;
                //RotateDir=0;
                //RotateSpeed=step_speed;
                break;
            case 4:
                mode=3;
                point=0;
                //RotateDir=1;
                //RotateSpeed=step_speed;
                break;
            case 5:
                mode=4;
                point=0;
                //RotateSpeed=step_speed*2;
                //RotateDir=0;
                break;
            case 6:
                mode=5; 
                point=1;
                RotateSpeed=step_speed*2;
                RotateDir=1; 
                break;
             case 7:
                
                break;               
                
            default:
                break;                                                                                                   
        }     
        button_event=0;            
    }  
}

//-----------------
void sensors_task()
{
 int status;
 char text[5];
 int32_t dif; 
 
    while (true) {
        ThisThread::flags_wait_any(0x1,true);
        SensorsEn=0;                 
        status = board->sensor_left->get_distance(&distance_l); 
        if (status != VL53L0X_ERROR_NONE)
            distance_l=8888;  
        status = board->sensor_right->get_distance(&distance_r);
        if (status != VL53L0X_ERROR_NONE)
             distance_r=8888;      
        status = board->sensor_centre->get_distance(&distance_c); 
        if (status != VL53L0X_ERROR_NONE)
             distance_c=8888; 
        switch(mode){
            case 0:
                sprintf(text,"c%3d",distance_c);  
                break;
            case 1:
                sprintf(text,"l%3d", distance_l);                        
                break;  
            case 2:      
                sprintf(text,"r%3d", distance_r);
                break;    
            case 3:
                dif=((int)distance_r-(int)distance_l);   
                sprintf(text,"d%3d",dif);
                if (abs(dif)>4) {
                    point=1;
                    if(dif>0)
                        RotateDir=1;
                    else
                        RotateDir=0;
                }else    
                    point=0;                       
                break;   
            case 4:      
                sprintf(text,"%d",G[0]);
                break;
            case 5:      
                sprintf(text,"%d",dur_time);
                break;                 
            default: 
                sprintf(text,"----");                    
                break;                      
        } 
        board->display->display_string(text);                                 
        SensorsEn=1;
   }         
}

/**********************************************************
*                    Interrupt Service Routines 
***********************************************************/
void proximityR_isr()
{
  prox=1;      
} 
//---------------------
void proximityF_isr()
{ 
  prox=0;      
}    

//---------------------
void polling_sensors_isr()
{
 if (SensorsEn){
    sensor_daemon.flags_set(0x1);
    led = !led; 
 }    
}
//---------------------------------
void clk_speed_isr()
{
    rotate_cnt++; 
    if (rotate_cnt>10)
        dur_time_en=1;
}