root / quad2 / arduino / src / ros_lib / examples / Temperature / Temperature.pde @ c1426757
History | View | Annotate | Download (1.83 KB)
1 |
/* |
---|---|
2 |
* rosserial Temperature Sensor Example |
3 |
* |
4 |
* This tutorial demonstrates the usage of the |
5 |
* Sparkfun TMP102 Digital Temperature Breakout board |
6 |
* http://www.sparkfun.com/products/9418 |
7 |
* |
8 |
* Source Code Based off of: |
9 |
* http://wiring.org.co/learning/libraries/tmp102sparkfun.html |
10 |
*/ |
11 |
|
12 |
#include <WProgram.h> |
13 |
#include <Wire.h> |
14 |
#include <ros.h> |
15 |
#include <std_msgs/Float32.h> |
16 |
|
17 |
ros::NodeHandle nh; |
18 |
|
19 |
|
20 |
std_msgs::Float32 temp_msg; |
21 |
ros::Publisher pub_temp("temperature", &temp_msg); |
22 |
|
23 |
|
24 |
// From the datasheet the BMP module address LSB distinguishes |
25 |
// between read (1) and write (0) operations, corresponding to |
26 |
// address 0x91 (read) and 0x90 (write). |
27 |
// shift the address 1 bit right (0x91 or 0x90), the Wire library only needs the 7 |
28 |
// most significant bits for the address 0x91 >> 1 = 0x48 |
29 |
// 0x90 >> 1 = 0x48 (72) |
30 |
|
31 |
int sensorAddress = 0x91 >> 1; // From datasheet sensor address is 0x91 |
32 |
// shift the address 1 bit right, the Wire library only needs the 7 |
33 |
// most significant bits for the address |
34 |
|
35 |
|
36 |
void setup() |
37 |
{ |
38 |
Wire.begin(); // join i2c bus (address optional for master) |
39 |
|
40 |
nh.initNode(); |
41 |
nh.advertise(pub_temp); |
42 |
|
43 |
} |
44 |
|
45 |
long publisher_timer; |
46 |
|
47 |
void loop() |
48 |
{ |
49 |
|
50 |
if (millis() > publisher_timer) { |
51 |
// step 1: request reading from sensor |
52 |
Wire.requestFrom(sensorAddress,2); |
53 |
delay(10); |
54 |
if (2 <= Wire.available()) // if two bytes were received |
55 |
{ |
56 |
byte msb; |
57 |
byte lsb; |
58 |
int temperature; |
59 |
|
60 |
msb = Wire.receive(); // receive high byte (full degrees) |
61 |
lsb = Wire.receive(); // receive low byte (fraction degrees) |
62 |
temperature = ((msb) << 4); // MSB |
63 |
temperature |= (lsb >> 4); // LSB |
64 |
|
65 |
temp_msg.data = temperature*0.0625; |
66 |
pub_temp.publish(&temp_msg); |
67 |
} |
68 |
|
69 |
publisher_timer = millis() + 1000; |
70 |
} |
71 |
|
72 |
nh.spinOnce(); |
73 |
} |