Integration of multiple sensors with ROS Serial

Dependencies:   mbed HCSR04 VEML7700

main.cpp

Committer:
zillkhan
Date:
2021-10-23
Revision:
3:c23614e6262f
Parent:
2:b847309a754d
Child:
4:69cd9240fd74

File content as of revision 3:c23614e6262f:

#include <ros.h>
#include <std_msgs/Float32.h>
#include <std_msgs/String.h>
#include <std_msgs/Int16.h>
#include "SHARPIR.h"
#include "hcsr04.h"
#include "VEML7700.h"

#ifndef MSU_VEML7700_I2C_ADDRESS
#define MSU_VEML7700_I2C_ADDRESS 0x10
#endif

#define PIN_SCL  D15
#define PIN_SDA  D14

//Sharp IR sensor analog pin defination
SHARPIR Sensor(A0); 

//Ultrasonic sensor pin defination
//D8 TRIGGER D9 ECHO
HCSR04 sensor(D8, D9);

ros::NodeHandle  nh;

//SharpIR publisher initialization
std_msgs::Float32 data_ir;
ros::Publisher sharpir("sharpir", &data_ir);

//Ultrasonic sensor publisher initialization
std_msgs::String data_ul;
ros::Publisher ultrasonic("ultrasonic", &data_ul);

//Ambient Light sensor publisher initialization
std_msgs::Int16 data_als;
ros::Publisher ALS("ALS", &data_als);

VEML7700 *veml7700 = 0 ;

int main() {
    float DistanceCM; //Variable to store distance in CM from SharpIR sensor
    char buffer[50]; // buffer for Ultrasonic sensor values
    
    //Initialization of Ambient light sensor
    uint16_t als ;
    veml7700 = new VEML7700(PIN_SDA, PIN_SCL, MSU_VEML7700_I2C_ADDRESS) ; 
    veml7700->setALSConf(0x0000) ;
    veml7700->setPowerSaving(0x0000) ;
    
    //Initalizing and advertising topics
    nh.initNode();
    nh.advertise(sharpir); //SharpIR sensor
    nh.advertise(ultrasonic); //Ultrasonic Sensor
    nh.advertise(ALS); //Ambient Light Sensor
    
    while (1) { //creates an eternal loop
    
        DistanceCM=Sensor.cm(); //Reading SharpIR sensor value from analog pin
        
        long distance = sensor.distance(); //Reading ultrasonic sensor value
        sprintf (buffer, "%d", distance);  //converting value into string
        
        als = veml7700->getALS(); //Readinng ALS sensor value
        
        data_ir.data = DistanceCM;
        sharpir.publish( &data_ir );
        
        data_ul.data = buffer;
        ultrasonic.publish( &data_ul );
        
        data_als.data = als;
        ALS.publish( &data_als );
        
        nh.spinOnce();
        wait_ms(1000);
    }
}