Sensors and motors work
Light sensor for ROS on Nucleo board
Dependencies: mbed ros_lib_kinetic
Revision 3:3a65e8aae6b6, committed 2019-10-15
- Comitter:
- florine_van
- Date:
- Tue Oct 15 12:20:06 2019 +0000
- Parent:
- 2:306c6c49a327
- Commit message:
- Code for two sensors
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Oct 15 09:07:24 2019 +0000
+++ b/main.cpp Tue Oct 15 12:20:06 2019 +0000
@@ -9,11 +9,13 @@
#include <std_msgs/Int32.h>
DigitalOut led = LED1;
+DigitalOut SHDN_1(D0);
// set-up serial to pc
I2C i2c(I2C_SDA, I2C_SCL);
// Set up I²C on the STM32 NUCLEO-401RE
-#define addr (0x52)
+#define addr1 (0x52)
+#define addr2 (0x60)
// I²C address of VL6180 shifted by 1 bit
//(0x29 << 1) so the R/W command can be added
@@ -21,7 +23,7 @@
// Split 16-bit register address into two bytes and write
// the address + data via I²C
///////////////////////////////////////////////////////////////////
-void WriteByte(wchar_t reg,char data) {
+void WriteByte(wchar_t reg,char data, char addr) {
char data_write[3];
data_write[0] = (reg >> 8) & 0xFF;;
// MSB of register address
@@ -35,7 +37,7 @@
// Split 16-bit register address into two bytes and write
// required register address to VL6180 and read the data back
///////////////////////////////////////////////////////////////////
-char ReadByte(wchar_t reg) {
+char ReadByte(wchar_t reg, char addr) {
char data_write[2];
char data_read[1];
@@ -50,59 +52,53 @@
///////////////////////////////////////////////////////////////////
// load settings
///////////////////////////////////////////////////////////////////
-int VL6180_Init() {
+int VL6180_Init(char addr) {
char reset;
- reset = ReadByte(0x016);
+
+ //WriteByte(0x212, addr/2, 0x52);
+
+
+ reset = ReadByte(0x016, addr);
if (reset==1)
{
// check to see has it be Initialised already
///////////////////////////////////////////////////////////////////
- // Added latest settings here - see Section 9
+ // SENSOR 1
///////////////////////////////////////////////////////////////////
// Mandatory : private registers
- WriteByte(0x0207, 0x01);
- WriteByte(0x0208, 0x01);
- WriteByte(0x0096, 0x00);
- WriteByte(0x0097, 0xfd);
- WriteByte(0x00e3, 0x01);
- WriteByte(0x00e4, 0x03);
- WriteByte(0x00e5, 0x02);
- WriteByte(0x00e6, 0x01);
- WriteByte(0x00e7, 0x03);
- WriteByte(0x00f5, 0x02);
- WriteByte(0x00d9, 0x05);
- WriteByte(0x00db, 0xce);
- WriteByte(0x00dc, 0x03);
- WriteByte(0x00dd, 0xf8);
- WriteByte(0x009f, 0x00);
- WriteByte(0x00a3, 0x3c);
- WriteByte(0x00b7, 0x00);
- WriteByte(0x00bb, 0x3c);
- WriteByte(0x00b2, 0x09);
- WriteByte(0x00ca, 0x09);
- WriteByte(0x0198, 0x01);
- WriteByte(0x01b0, 0x17);
- WriteByte(0x01ad, 0x00);
- WriteByte(0x00ff, 0x05);
- WriteByte(0x0100, 0x05);
- WriteByte(0x0199, 0x05);
- WriteByte(0x01a6, 0x1b);
- WriteByte(0x01ac, 0x3e);
- WriteByte(0x01a7, 0x1f);
- WriteByte(0x0030, 0x00);
+ WriteByte(0x0207, 0x01, addr);
+ WriteByte(0x0208, 0x01, addr);
+ WriteByte(0x0096, 0x00, addr);
+ WriteByte(0x0097, 0xfd, addr);
+ WriteByte(0x00e3, 0x01, addr);
+ WriteByte(0x00e4, 0x03, addr);
+ WriteByte(0x00e5, 0x02, addr);
+ WriteByte(0x00e6, 0x01, addr);
+ WriteByte(0x00e7, 0x03, addr);
+ WriteByte(0x00f5, 0x02, addr);
+ WriteByte(0x00d9, 0x05, addr);
+ WriteByte(0x00db, 0xce, addr);
+ WriteByte(0x00dc, 0x03, addr);
+ WriteByte(0x00dd, 0xf8, addr);
+ WriteByte(0x009f, 0x00, addr);
+ WriteByte(0x00a3, 0x3c, addr);
+ WriteByte(0x00b7, 0x00, addr);
+ WriteByte(0x00bb, 0x3c, addr);
+ WriteByte(0x00b2, 0x09, addr);
+ WriteByte(0x00ca, 0x09, addr);
+ WriteByte(0x0198, 0x01, addr);
+ WriteByte(0x01b0, 0x17, addr);
+ WriteByte(0x01ad, 0x00, addr);
+ WriteByte(0x00ff, 0x05, addr);
+ WriteByte(0x0100, 0x05, addr);
+ WriteByte(0x0199, 0x05, addr);
+ WriteByte(0x01a6, 0x1b, addr);
+ WriteByte(0x01ac, 0x3e, addr);
+ WriteByte(0x01a7, 0x1f, addr);
+ WriteByte(0x0030, 0x00, addr);
- /*
- // Recommended : Public registers - See data sheet for more detail
- WriteByte(0x0011, 0x10); // Enables polling for ‘New Sample ready’ when measurement completes
- WriteByte(0x010a, 0x30); // Set the averaging sample period (compromise between lower noise and increased execution time)
- WriteByte(0x003f, 0x46); // Sets the light and dark gain (upper // nibble). Dark gain should not be // changed.
- WriteByte(0x0031, 0xFF); // sets the # of range measurements after // which auto calibration of system is // performed
- WriteByte(0x0041, 0x63); // Set ALS integration time to 100ms
- WriteByte(0x002e, 0x01); // perform a single temperature calibration // of the ranging sensor
- */
-
- WriteByte(0x016, 0x00); //change fresh out of set status to 0
+ WriteByte(0x016, 0x00, addr); //change fresh out of set status to 0
}
return 0;
}
@@ -110,26 +106,27 @@
///////////////////////////////////////////////////////////////////
// Start a range measurement in single shot mode
///////////////////////////////////////////////////////////////////
-int VL6180_Start_Range() {
- WriteByte(0x018,0x03);
+int VL6180_Start_Range(char addr) {
+ WriteByte(0x018,0x03, addr);
return 0;
}
///////////////////////////////////////////////////////////////////
// poll for new sample ready ready
///////////////////////////////////////////////////////////////////
-int VL6180_Poll_Range() {
+
+int VL6180_Poll_Range(char addr) {
char status;
char range_status;
// check the status
- status = ReadByte(0x04f);
+ status = ReadByte(0x04f,addr);
range_status = status & 0x07;
// wait for new measurement ready status
- while (range_status != 0x04)
+ while (range_status != 0x00)
{
- status = ReadByte(0x04f);
+ status = ReadByte(0x04f,addr);
range_status = status & 0x07;
wait_ms(1); // (can be removed)
}
@@ -137,12 +134,13 @@
return 0;
}
+
///////////////////////////////////////////////////////////////////
// Read range result (mm)
///////////////////////////////////////////////////////////////////
-int VL6180_Read_Range() {
+int VL6180_Read_Range(char addr) {
int range;
- range=ReadByte(0x062);
+ range=ReadByte(0x062, addr);
return range;
}
@@ -150,7 +148,8 @@
// clear interrupts
///////////////////////////////////////////////////////////////////
int VL6180_Clear_Interrupts() {
- WriteByte(0x015,0x07);
+ WriteByte(0x015,0x07, addr1);
+ WriteByte(0x015,0x07, addr2);
return 0;
}
@@ -162,29 +161,51 @@
ros::NodeHandle nh;
nh.initNode();
- std_msgs::Int32 int_msg;
- ros::Publisher range_pub("range", &int_msg);
+ std_msgs::Int32 int_sensor1_msg;
+ std_msgs::Int32 int_sensor2_msg;
+ ros::Publisher range1_pub("sensor1", &int_sensor1_msg);
+ ros::Publisher range2_pub("sensor2", &int_sensor2_msg);
- nh.advertise(range_pub);
+ nh.advertise(range1_pub);
+ nh.advertise(range2_pub);
+
+ int range1;
+ int range2;
+
+ SHDN_1 = 0;
- int range;
+ //change default address of sensor 2
+ WriteByte(0x212, 0x30, addr1);
+
+ SHDN_1 = 1;
+
+ //SHDN_1 = 1;
// load settings onto VL6180X
- VL6180_Init();
+ VL6180_Init(addr1);
+ //SHDN_1 = 0;
+ VL6180_Init(addr2);
while (1)
{
// start range measurement
- VL6180_Start_Range();
- // poll the VL6180 till new sample ready
- //VL6180_Poll_Range();
- // read range result
- range = VL6180_Read_Range();
+
+ // poll the VL6180 till new sample ready
+ VL6180_Start_Range(addr1);
+ VL6180_Poll_Range(addr1);
+ range1 = VL6180_Read_Range(addr1);
+
+ // read range result
+ VL6180_Start_Range(addr2);
+ VL6180_Poll_Range(addr2);
+ range2 = VL6180_Read_Range(addr2);
// clear the interrupt on VL6180
VL6180_Clear_Interrupts();
// send range to pc by serial
- int_msg.data = range;
- range_pub.publish(&int_msg);
+ int_sensor1_msg.data = range1;
+ int_sensor2_msg.data = range2;
+ range1_pub.publish(&int_sensor1_msg);
+ range2_pub.publish(&int_sensor2_msg);
nh.spinOnce();
wait(0.1);