This program is porting rosserial_arduino for mbed http://www.ros.org/wiki/rosserial_arduino This program supported the revision of 169 of rosserial. This program contains an example.
Dependencies: rosserial_mbed_lib mbed Servo
examples/Temperature.cpp@0:06fc856e99ca, 2011-08-19 (annotated)
- Committer:
- nucho
- Date:
- Fri Aug 19 09:06:16 2011 +0000
- Revision:
- 0:06fc856e99ca
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nucho | 0:06fc856e99ca | 1 | //#define COMPILE_TEMPERATURE_CODE_ROSSERIAL |
nucho | 0:06fc856e99ca | 2 | #ifdef COMPILE_TEMPERATURE_CODE_ROSSERIAL |
nucho | 0:06fc856e99ca | 3 | |
nucho | 0:06fc856e99ca | 4 | /* |
nucho | 0:06fc856e99ca | 5 | * rosserial Temperature Sensor Example |
nucho | 0:06fc856e99ca | 6 | * |
nucho | 0:06fc856e99ca | 7 | * This tutorial demonstrates the usage of the |
nucho | 0:06fc856e99ca | 8 | * Sparkfun TMP102 Digital Temperature Breakout board |
nucho | 0:06fc856e99ca | 9 | * http://www.sparkfun.com/products/9418 |
nucho | 0:06fc856e99ca | 10 | * |
nucho | 0:06fc856e99ca | 11 | * Source Code Based off of: |
nucho | 0:06fc856e99ca | 12 | * http://wiring.org.co/learning/libraries/tmp102sparkfun.html |
nucho | 0:06fc856e99ca | 13 | */ |
nucho | 0:06fc856e99ca | 14 | |
nucho | 0:06fc856e99ca | 15 | #include "mbed.h" |
nucho | 0:06fc856e99ca | 16 | #include <ros.h> |
nucho | 0:06fc856e99ca | 17 | #include <std_msgs/Float32.h> |
nucho | 0:06fc856e99ca | 18 | |
nucho | 0:06fc856e99ca | 19 | ros::NodeHandle nh; |
nucho | 0:06fc856e99ca | 20 | |
nucho | 0:06fc856e99ca | 21 | std_msgs::Float32 temp_msg; |
nucho | 0:06fc856e99ca | 22 | ros::Publisher pub_temp("temperature", &temp_msg); |
nucho | 0:06fc856e99ca | 23 | |
nucho | 0:06fc856e99ca | 24 | |
nucho | 0:06fc856e99ca | 25 | // From the datasheet the BMP module address LSB distinguishes |
nucho | 0:06fc856e99ca | 26 | // between read (1) and write (0) operations, corresponding to |
nucho | 0:06fc856e99ca | 27 | // address 0x91 (read) and 0x90 (write). |
nucho | 0:06fc856e99ca | 28 | // shift the address 1 bit right (0x91 or 0x90), the Wire library only needs the 7 |
nucho | 0:06fc856e99ca | 29 | // most significant bits for the address 0x91 >> 1 = 0x48 |
nucho | 0:06fc856e99ca | 30 | // 0x90 >> 1 = 0x48 (72) |
nucho | 0:06fc856e99ca | 31 | |
nucho | 0:06fc856e99ca | 32 | int sensorAddress = 0x91 >>1; // From datasheet sensor address is 0x91 |
nucho | 0:06fc856e99ca | 33 | |
nucho | 0:06fc856e99ca | 34 | // shift the address 1 bit right, the Wire library only needs the 7 |
nucho | 0:06fc856e99ca | 35 | // most significant bits for the address |
nucho | 0:06fc856e99ca | 36 | |
nucho | 0:06fc856e99ca | 37 | Timer t; |
nucho | 0:06fc856e99ca | 38 | I2C i2c(p9, p10); // sda, scl |
nucho | 0:06fc856e99ca | 39 | |
nucho | 0:06fc856e99ca | 40 | int main() { |
nucho | 0:06fc856e99ca | 41 | t.start(); |
nucho | 0:06fc856e99ca | 42 | |
nucho | 0:06fc856e99ca | 43 | nh.initNode(); |
nucho | 0:06fc856e99ca | 44 | nh.advertise(pub_temp); |
nucho | 0:06fc856e99ca | 45 | |
nucho | 0:06fc856e99ca | 46 | long publisher_timer =0; |
nucho | 0:06fc856e99ca | 47 | |
nucho | 0:06fc856e99ca | 48 | while (1) { |
nucho | 0:06fc856e99ca | 49 | |
nucho | 0:06fc856e99ca | 50 | if (t.read_ms() > publisher_timer) { |
nucho | 0:06fc856e99ca | 51 | // step 1: request reading from sensor |
nucho | 0:06fc856e99ca | 52 | //Wire.requestFrom(sensorAddress,2); |
nucho | 0:06fc856e99ca | 53 | char cmd = 2; |
nucho | 0:06fc856e99ca | 54 | i2c.write(sensorAddress, &cmd, 1); |
nucho | 0:06fc856e99ca | 55 | |
nucho | 0:06fc856e99ca | 56 | wait_ms(50); |
nucho | 0:06fc856e99ca | 57 | |
nucho | 0:06fc856e99ca | 58 | char msb; |
nucho | 0:06fc856e99ca | 59 | char lsb; |
nucho | 0:06fc856e99ca | 60 | int temperature; |
nucho | 0:06fc856e99ca | 61 | i2c.read(sensorAddress, &msb, 1); // receive high byte (full degrees) |
nucho | 0:06fc856e99ca | 62 | i2c.read(sensorAddress, &lsb, 1); // receive low byte (fraction degrees) |
nucho | 0:06fc856e99ca | 63 | |
nucho | 0:06fc856e99ca | 64 | temperature = ((msb) << 4); // MSB |
nucho | 0:06fc856e99ca | 65 | temperature |= (lsb >> 4); // LSB |
nucho | 0:06fc856e99ca | 66 | |
nucho | 0:06fc856e99ca | 67 | temp_msg.data = temperature*0.0625; |
nucho | 0:06fc856e99ca | 68 | pub_temp.publish(&temp_msg); |
nucho | 0:06fc856e99ca | 69 | |
nucho | 0:06fc856e99ca | 70 | |
nucho | 0:06fc856e99ca | 71 | publisher_timer = t.read_ms() + 1000; |
nucho | 0:06fc856e99ca | 72 | } |
nucho | 0:06fc856e99ca | 73 | |
nucho | 0:06fc856e99ca | 74 | nh.spinOnce(); |
nucho | 0:06fc856e99ca | 75 | } |
nucho | 0:06fc856e99ca | 76 | } |
nucho | 0:06fc856e99ca | 77 | |
nucho | 0:06fc856e99ca | 78 | #endif |