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-09-29
Revision:
6:f253ca7919af
Parent:
5:fa65d931bd96
Child:
7:865f0c23a183

File content as of revision 6:f253ca7919af:

#include "mbed.h"
#include "vl6180x_class.h"
#include "STMPE1600_class.h"

#include <string.h>
#include <stdlib.h>
#include <stdio.h>

#define VL6180X_I2C_SDA I2C_SDA
#define VL6180X_I2C_SCL I2C_SCL


/* Enable e disable function -------------------------------------------------------*/
#define EXPANDER_I2C_ADDRESS    (0x42*2)
#define GPMR    0x10
#define GPSR    0x12
#define GPDR    0x14

#define V2_CHIPEN   (1<<12)
#define V2_CHIPEN_B (1<<13)
#define V2_CHIPEN_L (1<<14)
#define V2_CHIPEN_R (1<<15)

/*----------------------------------------------------------------------------------*/	
	
void OnErrLog(void){};
void DISP_ExecLoopBody(void){};

int main()
{   
	  uint8_t buf[8]={1,1,0,0,0,0,0,0};
    static uint32_t tmp;

		uint8_t x=0xFF;
		uint8_t y=0xF7;
		uint8_t z=0x80;
		uint8_t k=0x17;
		uint8_t g=0xE7;
		
		DigitalOut  led (LED2);
		led=1;
		
		DevI2C device_i2c(VL6180X_I2C_SDA, VL6180X_I2C_SCL);

    STMPE1600DigiOut gpio0(device_i2c, GPIO_0, STMPE1600_DEF_DEVICE_ADDRESS);					
    STMPE1600DigiOut gpio1(device_i2c, GPIO_1, STMPE1600_DEF_DEVICE_ADDRESS);		
    STMPE1600DigiOut gpio2(device_i2c, GPIO_2, STMPE1600_DEF_DEVICE_ADDRESS);			
    STMPE1600DigiOut gpio3(device_i2c, GPIO_3, STMPE1600_DEF_DEVICE_ADDRESS);		
    STMPE1600DigiOut gpio4(device_i2c, GPIO_4, STMPE1600_DEF_DEVICE_ADDRESS);				
    STMPE1600DigiOut gpio5(device_i2c, GPIO_5, STMPE1600_DEF_DEVICE_ADDRESS);		
    STMPE1600DigiOut gpio6(device_i2c, GPIO_6, STMPE1600_DEF_DEVICE_ADDRESS);		
    STMPE1600DigiOut gpio7(device_i2c, GPIO_7, STMPE1600_DEF_DEVICE_ADDRESS);		
    STMPE1600DigiOut gpio8(device_i2c, GPIO_8, STMPE1600_DEF_DEVICE_ADDRESS);				

    STMPE1600DigiOut gpio9(device_i2c, GPIO_9, STMPE1600_DEF_DEVICE_ADDRESS);		
    STMPE1600DigiOut gpio10(device_i2c, GPIO_10, STMPE1600_DEF_DEVICE_ADDRESS);			
    STMPE1600DigiIn  gpio11(device_i2c, GPIO_11, STMPE1600_DEF_DEVICE_ADDRESS);		
    STMPE1600DigiOut gpio12(device_i2c, GPIO_12, STMPE1600_DEF_DEVICE_ADDRESS);				
    STMPE1600DigiOut gpio13(device_i2c, GPIO_13, STMPE1600_DEF_DEVICE_ADDRESS);		
    STMPE1600DigiOut gpio14(device_i2c, GPIO_14, STMPE1600_DEF_DEVICE_ADDRESS);		
    STMPE1600DigiOut gpio15(device_i2c, GPIO_15, STMPE1600_DEF_DEVICE_ADDRESS);		



		gpio0 = 0; gpio1 = 1; gpio2 = 1; gpio3 = 1; gpio4 = 1;	gpio5 = 1; gpio6 = 1; gpio7 = 0;
		gpio8 = 1; gpio9 = 1; gpio10 = 1; gpio12 = 1;	gpio13 = 1; gpio14 = 1; gpio15 = 1;		
		
//STMPE1600DigiOut * p_exp = new STMPE1600DigiOut(device_i2c, GPIO_12, STMPE1600_DEF_DEVICE_ADDRESS);		
		
	  VL6180X sensor1(device_i2c, gpio12);
		
		sensor1.VL6180x_On();
//		tmp = sensor1.Init();
		
	uint8_t id=0;
		id = sensor1.ReadID();   // read id to test 
		
//	  VL6180X sensor1(device_i2c, exp);		
			
		
		while (1);
		 
	  //imposto GPIO di input e output
		device_i2c.i2c_write(&x, 0x42*2, 0x14, 1);
		device_i2c.i2c_write(&y, 0x42*2, 0x15, 1);
		//disabilito i display e abilito il GPIO0 del sensore top
		device_i2c.i2c_write(&z, 0x42*2, 0x12, 1);
		device_i2c.i2c_write(&g, 0x42*2, 0x13, 1); //spengo gpio0 
		device_i2c.i2c_write(&k, 0x42*2, 0x13, 1); //accendo gpio0
		
		//VL6180x_I2CRead(0x29, (uint16_t)0x000, buf, 2);
		//VL6180x_I2CRead(0x29, (uint16_t)0x212, buf+1, 2);
		//buf[2]=0x32;
		//VL6180x_I2CWrite(0x29, (uint16_t)0x0212, buf+2, 1);
		//VL6180x_I2CRead(0x32, (uint16_t)0x212, buf+3, 2);
		
    buf[0]=sensor1.Init();
    buf[1]=sensor1.ReadID(); 
		//buf[2]=sensor1.Prepare();
		//sensor1.~VL6180X();
		

		/*
		X_NUCLEO_6180XA1 board;
	
		status=board.vl6180x_top.RdByte(IDENTIFICATION_MODEL_ID, &buffer);
	  if(!status)
		{
		    board.vl6180x_top.SetPresent();
	   		board.vl6180x_top.SetDeviceID(i);
			  if(!(board.vl6180x_top.Init()||board.vl6180x_top.Prepare()))
				{
				    board.vl6180x_top.SetReady();
						status=board.vl6180x_top.SetI2CAddress(final_i2c_address);
						if(status)
						    errore su scrittura indirizzo i2c
				}
				else
				    errore su init o prepare
		}		
		i++;
		
		//stessa cosa per gli altri sensori (left, bottom e right)
		   
		*/

}