Sweep a servo according to Proximity sensor measure

Dependencies:   Servo X_NUCLEO_6180XA1 mbed

Fork of HelloWorld_6180XA1 by ST

main.cpp

Committer:
gallonm
Date:
2015-11-05
Revision:
27:2afb9baf718f
Parent:
26:e19e6cb2e6bb
Child:
30:c8efec21544f

File content as of revision 27:2afb9baf718f:

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

/* This test application performs a range measurement and an als measurement. 
   Display select slider allow to switch the measurement type; previuos 
   measurement is stopped and a new measurement is started.
   User button allows to stop current measurement and the entire program.
   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 flag_sensor_top=false, flag_stop_measure=false;	

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

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

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);
         board->display->DisplayString(str, strlen(str));
      }
      else
      {
         sprintf(str,"%s","----");
         board->display->DisplayString(str, strlen(str));
      }
   }
   else if(op_mode==als_continuous_interrupt)
   {
      if(data_sensor_top.lux!=0xFFFFFFFF)
      {
         sprintf(str,"%d",data_sensor_top.lux);
         board->display->DisplayString(str, strlen(str));
      }
      else
      {
         sprintf(str,"%s","----");
         board->display->DisplayString(str, strlen(str));
      }
   }    
}

OperatingMode CheckSlider(int measure)
{
   if(measure==RANGE)
      return range_continuous_interrupt;
   else if(measure==ALS)
      return als_continuous_interrupt;
}

void StartPrintMessage(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");
}

void StopPrintMessage(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");
}

void Bye()
{
   int i;
   char str[5];
   
   for(i=0;i<200;i++)
   {
      sprintf(str,"%s","BYE");
      board->display->DisplayString(str, strlen(str));
   }
}

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, measure;
 
   //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);
   measure=measure_type;
   operating_mode=CheckSlider(measure);
   status=board->sensor_top->StartMeasurement(operating_mode, SensorTopIRQ, NULL, NULL);
   if(!status)
   {
      prev_operating_mode=operating_mode;
      StartPrintMessage(operating_mode);
      while(1)
      {
         if(flag_sensor_top)
         {
            flag_sensor_top=false;
            status=board->sensor_top->HandleIRQ(operating_mode, &data_sensor_top);
            DisplayRefresh(operating_mode);
         }
         if(flag_stop_measure)
         {
            status=board->sensor_top->StopMeasurement(prev_operating_mode);
            if(!status)
               StopPrintMessage(prev_operating_mode);
            printf("\nProgram stopped!\n\n\r");
            break;
         }
         measure=measure_type;
         operating_mode=CheckSlider(measure);
         if(operating_mode!=prev_operating_mode)
         {
            DisplayRefresh(prev_operating_mode);
            status=board->sensor_top->StopMeasurement(prev_operating_mode);
            if(!status)
               StopPrintMessage(prev_operating_mode);
            prev_operating_mode=operating_mode;
            status=board->sensor_top->StartMeasurement(operating_mode, SensorTopIRQ, NULL, NULL);
            if(!status)
               StartPrintMessage(operating_mode);
         }
         else
            DisplayRefresh(operating_mode);
      }
   }
   Bye();

   delete board;
}