Temperature.cpp
Go to the documentation of this file.
00001 /*
00002  * rosserial Temperature Sensor Example
00003  *
00004  * This tutorial demonstrates the usage of the
00005  * Sparkfun TMP102 Digital Temperature Breakout board
00006  * http://www.sparkfun.com/products/9418
00007  *
00008  * Source Code Based off of:
00009  * http://wiring.org.co/learning/libraries/tmp102sparkfun.html
00010  */
00011 
00012 #include "mbed.h"
00013 #include <ros.h>
00014 #include <std_msgs/Float32.h>
00015 
00016 ros::NodeHandle  nh;
00017 
00018 std_msgs::Float32 temp_msg;
00019 ros::Publisher pub_temp("temperature", &temp_msg);
00020 
00021 
00022 // From the datasheet the BMP module address LSB distinguishes
00023 // between read (1) and write (0) operations, corresponding to
00024 // address 0x91 (read) and 0x90 (write).
00025 // shift the address 1 bit right (0x91 or 0x90), the Wire library only needs the 7
00026 // most significant bits for the address 0x91 >> 1 = 0x48
00027 // 0x90 >> 1 = 0x48 (72)
00028 
00029 int sensorAddress = 0x91 >>1;  // From datasheet sensor address is 0x91
00030                                // shift the address 1 bit right, the Wire library only needs the 7
00031                                // most significant bits for the address
00032 
00033 Timer t;
00034 #ifdef TARGET_LPC1768
00035 I2C i2c(p9, p10);        // sda, scl
00036 #elif defined(TARGET_KL25Z) || defined(TARGET_NUCLEO_F401RE)
00037 I2C i2c(D14, D15);       // sda, scl
00038 #else
00039 #error "You need to specify a pin for the sensor"
00040 #endif
00041 
00042 int main() {
00043     t.start();
00044 
00045     nh.initNode();
00046     nh.advertise(pub_temp);
00047 
00048     long publisher_timer =0;
00049 
00050     while (1) {
00051 
00052         if (t.read_ms() > publisher_timer) {
00053             // step 1: request reading from sensor
00054             //Wire.requestFrom(sensorAddress,2);
00055             char cmd = 2;
00056             i2c.write(sensorAddress, &cmd, 1);
00057 
00058             wait_ms(50);
00059 
00060             char msb;
00061             char lsb;
00062             int temperature;
00063             i2c.read(sensorAddress, &msb, 1); // receive high byte (full degrees)
00064             i2c.read(sensorAddress, &lsb, 1); // receive low byte (fraction degrees)
00065 
00066             temperature = ((msb) << 4);  // MSB
00067             temperature |= (lsb >> 4);   // LSB
00068 
00069             temp_msg.data = temperature*0.0625;
00070             pub_temp.publish(&temp_msg);
00071 
00072             publisher_timer = t.read_ms() + 1000;
00073         }
00074 
00075         nh.spinOnce();
00076     }
00077 }
00078 


rosserial_mbed
Author(s): Gary Servin
autogenerated on Sat Oct 7 2017 03:08:46