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:
2015-11-09
Revision:
30:c8efec21544f
Parent:
27:2afb9baf718f
Child:
31:1aaf6b8b066e

File content as of revision 30:c8efec21544f:

#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)
   {
      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)
   {
      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 */
OperatingMode CheckSlider(STMPE1600DigiIn measure_type)
{
   int measure=measure_type;   
   if(measure==RANGE)
      return range_continuous_interrupt;
   else if(measure==ALS)
      return als_continuous_interrupt;
}

/* 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
void Bye()
{
   Timer timer;
   char str[5];
   
   timer.start();
   for(int i=0; i<DELAY; i=timer.read_ms())
   {
      sprintf(str,"%s","BYE");
      board->display->DisplayString(str, strlen(str));
   }
   timer.stop();
}

int main()
{   
   DevI2C device_i2c(VL6180X_I2C_SDA, VL6180X_I2C_SCL);
   board=X_NUCLEO_6180XA1::Instance(&device_i2c);
   /* user button used to stop measurements in interrupt mode */
   InterruptIn stop_button(USER_BUTTON);
   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
   
   status=board->InitBoard();
   if(status)
      printf("Failed to init board!\n\r");   
   
   stop_button.rise(&StopMeasureIRQ);
   operating_mode=CheckSlider(measure_type);   
   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);
            printf("\nProgram stopped!\n\n\r");
            break;
         }
         operating_mode=CheckSlider(measure_type);
         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);
      }
   }
   Bye();

   delete board;
}