Sweep a servo according to Proximity sensor measure

Dependencies:   Servo X_NUCLEO_6180XA1 mbed

Fork of HelloWorld_6180XA1 by ST

main.cpp

Committer:
mapellil
Date:
2016-03-15
Revision:
40:204f84887d70
Parent:
39:851cfabf7aa6
Child:
42:ee8c5b1f2e1d

File content as of revision 40:204f84887d70:

#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 IRQ 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   D14 
#define VL6180X_I2C_SCL   D15 

#define RANGE   0
#define ALS     1

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(enum OpModeIntPoll_t OpMode) {
	
OperatingMode ret;
int measure= board->RdSwitch();

   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 indicated message <= 4 char */
#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) {
   int status; 	 
   /* creates the 6180XA1 expansion board singleton obj */
   board=X_NUCLEO_6180XA1::Instance(device_i2c, A3, A2, D13, D2);
   DisplayMsg  ("INT");
   /* init the 6180XA1 expansion board with default values */
   status=board->InitBoard();
   if(status)
      printf("Failed to init board!\n\r");   
   /* check the red slider position for ALS/Range measure */
   operating_mode=CheckSlider(IntMeasure);   
   /* start the measure on sensor top */
   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)  /* 6180 isr was triggered */
         {
            int_sensor_top=false;
            status=board->sensor_top->HandleIRQ(operating_mode, &data_sensor_top); /* handle the isr and read the meaure */
            DisplayRefresh(operating_mode);
         }
         if(int_stop_measure) /* Blue Button isr was triggered */
         {
            status=board->sensor_top->StopMeasurement(prev_operating_mode); /* stop the measure and exit */
            if(!status)
               PrintStopMessage(prev_operating_mode);
			int_stop_measure = false;
            printf("\nProgram stopped!\n\n\r");
            break;
         }
         operating_mode=CheckSlider(IntMeasure); /* check if red slider was moved */
         if(operating_mode!=prev_operating_mode)
         {
            DisplayRefresh(prev_operating_mode);
            status=board->sensor_top->StopMeasurement(prev_operating_mode); /* stop the running measure */
            if(!status)
               PrintStopMessage(prev_operating_mode);
            prev_operating_mode=operating_mode;
            status=board->sensor_top->StartMeasurement(operating_mode, SensorTopIRQ, NULL, NULL); /* start the new measure */
            if(!status)
               PrintStartMessage(operating_mode);
         } else
            DisplayRefresh(operating_mode);           
      }
   }
   DisplayMsg("BYE");
   delete board;	    
}	 

/*=================================== Main ==================================
 Move the VL6180X Expansion board red slider to switch between ALS or Range
 measures.
 Press the blue user button to stop the measurements in progress 	
=============================================================================*/
int main()
{   
#if USER_BUTTON==PC_13  // we are cross compiling for Nucleo-f401 
   InterruptIn stop_button (USER_BUTTON);
   stop_button.rise (&StopMeasureIRQ);	
#endif   
   DevI2C *device_i2c =new DevI2C(VL6180X_I2C_SDA, VL6180X_I2C_SCL);     
		
   IntContinousALSorRangeMeasure (device_i2c);  // start continous measures Interrupt based
}