Sweep a servo according to Proximity sensor measure

Dependencies:   Servo X_NUCLEO_6180XA1 mbed

Fork of HelloWorld_6180XA1 by ST

main.cpp

Committer:
licio.mapelli@st.com
Date:
2015-11-12
Revision:
32:724d2afb0ca2
Parent:
31:1aaf6b8b066e
Child:
33:b903a0ec8803

File content as of revision 32:724d2afb0ca2:

#include "mbed.h"
#include "x_nucleo_6180xa1.h"
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <assert.h>

/* This VL6180X Expansion board test application performs a range measurement and an als measurement in interrupt mode
   on the onboard embedded top sensor. 
   The board red slider select on the flight the measurement type as ALS or RANGE; the measured data is diplayed on the 
   on bord 4digits display.

   User Blue button allows to stop current measurement and the entire program releasing all the resources.
   Reset button is used to restart the program. */

/* Polling operating modes don`t require callback function that handles IRQ 
   Callback functions are used only for measure that require interrupt */

/* GetMeasurement is asynchronous! It returns NOT_READY if the measurement value 
   is not ready to be read from the corresponding register. So you need to wait
   for the result to be ready */

#define VL6180X_I2C_SDA I2C_SDA
#define VL6180X_I2C_SCL I2C_SCL

#define RANGE   0
#define ALS     1

/* timer and digital out pin used for debugging */
//Timer timer;
//int start, end; 
//DigitalOut pin(PA_15); 

static X_NUCLEO_6180XA1 *board=NULL;
MeasureData_t data_sensor_top;
OperatingMode operating_mode, prev_operating_mode;
	
/* flags that handle interrupt request */
bool int_sensor_top=false, int_stop_measure=false;	

/* ISR callback function of the sensor_top */
void SensorTopIRQ(void)
{
   int_sensor_top=true;
   board->sensor_top->DisableInterruptMeasureDetectionIRQ();
}	

/* ISR callback function of the user blue button to stop program */
void StopMeasureIRQ(void)
{
   int_stop_measure=true;
}

/* On board 4 digit local display refresh */
void DisplayRefresh(OperatingMode op_mode)
{   
   char str[5];
   
   if(op_mode==range_continuous_interrupt || op_mode==range_continuous_polling)
   {
      if(data_sensor_top.range_mm!=0xFFFFFFFF)
      {
         sprintf(str,"%d",data_sensor_top.range_mm);
      }
      else
      {
         sprintf(str,"%s","----");
      }
   }
   else if(op_mode==als_continuous_interrupt || op_mode==als_continuous_polling)
   {
      if(data_sensor_top.lux!=0xFFFFFFFF)
      {
         sprintf(str,"%d",data_sensor_top.lux);
      }
      else
      {
         sprintf(str,"%s","----");
      }
   }
   board->display->DisplayString(str, strlen(str));       
}

/* On board red slider position check */
enum OpModeIntPoll_t{ PollMeasure, IntMeasure };

OperatingMode CheckSlider(STMPE1600DigiIn measure_type, enum OpModeIntPoll_t OpMode)
{
int measure=measure_type;   

OperatingMode ret;
   switch (OpMode) {
   	case PollMeasure:
      if(measure==RANGE)
        ret = range_continuous_polling;
      else if(measure==ALS)
        ret = als_continuous_polling;   	   	
   	break;
   	
   	case IntMeasure:
      if(measure==RANGE)
        ret = range_continuous_interrupt;
      else if(measure==ALS)
        ret = als_continuous_interrupt;   	
   	break;
   }
	 return ret;
}

/* Print on USB Serial the started OperatingMode */
void PrintStartMessage(OperatingMode op_mode)
{
   if(op_mode==range_continuous_interrupt)
      printf("\nStarted range continuous interrupt measure\n\r");
   else if(prev_operating_mode==als_continuous_interrupt)
      printf("\nStarted als continuous interrupt measure\n\r");
}

/* Print on USB Serial the stopped OperatingMode */
void PrintStopMessage(OperatingMode op_mode)
{
   if(op_mode==range_continuous_interrupt)
      printf("Stopped range continuous interrupt measure\n\r");
   else if(prev_operating_mode==als_continuous_interrupt)
      printf("Stopped als continuous interrupt measure\n\r");
}

/* Print on board 4 Digit display the app exiting BYE message */
#define DELAY 2000  // 2Sec
void DisplayMsg(const char * msg)
{
   Timer timer;
   char str[5];
   
   timer.start();
   for(int i=0; i<DELAY; i=timer.read_ms())
   {
      sprintf(str,"%s",msg);
      board->display->DisplayString(str, strlen(str));
   }
   timer.stop();
}


void IntContinousALSorRangeMeasure (DevI2C device_i2c) {
	 // onboard red slider to switch Range/ALS measures 
   STMPE1600DigiIn measure_type(device_i2c, GPIO_11);
   int status; 
   //device_i2c.frequency(400000); //change i2c frequncy from 100kHz to 400kHz
   //pc.baud(115200); //change the printf baudrate if faster printf is needed
	
	 board=X_NUCLEO_6180XA1::Instance(&device_i2c);
   DisplayMsg  ("INT");
	
   status=board->InitBoard();
   if(status)
      printf("Failed to init board!\n\r");   
   

   operating_mode=CheckSlider(measure_type, IntMeasure);   
   status=board->sensor_top->StartMeasurement(operating_mode, SensorTopIRQ, NULL, NULL);
   if(!status)
   {
      prev_operating_mode=operating_mode;
      PrintStartMessage(operating_mode);
      while(1)
      {
         if(int_sensor_top)
         {
            int_sensor_top=false;
            status=board->sensor_top->HandleIRQ(operating_mode, &data_sensor_top);
            DisplayRefresh(operating_mode);
         }
         if(int_stop_measure)
         {
            status=board->sensor_top->StopMeasurement(prev_operating_mode);
            if(!status)
               PrintStopMessage(prev_operating_mode);
						int_stop_measure = false;
            printf("\nProgram stopped!\n\n\r");
            break;
         }
         operating_mode=CheckSlider(measure_type, IntMeasure);
         if(operating_mode!=prev_operating_mode)
         {
            DisplayRefresh(prev_operating_mode);
            status=board->sensor_top->StopMeasurement(prev_operating_mode);
            if(!status)
               PrintStopMessage(prev_operating_mode);
            prev_operating_mode=operating_mode;
            status=board->sensor_top->StartMeasurement(operating_mode, SensorTopIRQ, NULL, NULL);
            if(!status)
               PrintStartMessage(operating_mode);
         } else
            DisplayRefresh(operating_mode);           
      }
   }
   DisplayMsg("BYE");
   delete board;	 
}	 


void PollContinousALSorRangeMeasure (DevI2C device_i2c) {
	 // onboard red slider to switch Range/ALS measures 
   STMPE1600DigiIn measure_type(device_i2c, GPIO_11);  
   int status; 

   board=X_NUCLEO_6180XA1::Instance(&device_i2c);
   DisplayMsg  ("poll");	 
   status=board->InitBoard();
   if(status)
      printf("Failed to init board!\n\r");      
   operating_mode=CheckSlider(measure_type, PollMeasure);   
   status=board->sensor_top->StartMeasurement(operating_mode, NULL, NULL, NULL);
   if(!status)
   {
      prev_operating_mode=operating_mode;
      PrintStartMessage(operating_mode);
      while(1)
      {         
//	       do   // the loop waiting for the result could be avoided (if is acceptable to get not updated measures) 
//    	   {
        	  status=board->sensor_top->GetMeasurement(operating_mode, &data_sensor_top);
						DisplayRefresh(operating_mode);					 
//         }while(status);

         if(int_stop_measure)
         {
            status=board->sensor_top->StopMeasurement(prev_operating_mode);
            if(!status)
               PrintStopMessage(prev_operating_mode);
						int_stop_measure = false;						
            printf("\nProgram stopped!\n\n\r");
            break;
         }
 
         operating_mode=CheckSlider(measure_type, PollMeasure);
         if(operating_mode!=prev_operating_mode)
         {
            DisplayRefresh(prev_operating_mode);
            status=board->sensor_top->StopMeasurement(prev_operating_mode);
            if(!status)
               PrintStopMessage(prev_operating_mode);
            prev_operating_mode=operating_mode;
            status=board->sensor_top->StartMeasurement(operating_mode, NULL, NULL, NULL);
            if(!status)
               PrintStartMessage(operating_mode);
         }
         else
            DisplayRefresh(operating_mode);
      }      
   }
   DisplayMsg("BYE");
   delete board;	 
}


int main()
{   
   /* user button to stop measurements in progress */	
   InterruptIn stop_button (USER_BUTTON);
   stop_button.rise (&StopMeasureIRQ);	
   DevI2C device_i2c (VL6180X_I2C_SDA, VL6180X_I2C_SCL);
		
   IntContinousALSorRangeMeasure (device_i2c);  // start Interrupt continous measures
   
   PollContinousALSorRangeMeasure (device_i2c); // start Polling continous measures

}