main.cpp

00001 #include "mbed.h"
00002 #include <ros.h>
00003 #include <ros/time.h>
00004 #include <std_msgs/Int16.h>
00005 #define PCF8574_ADDR     (0x40)
00006 #define PCF8574_ADDR_1     (0x40) 
00007 #define PCF8574_ADDR_2     (0x42) 
00008 
00009 I2C i2c(PB_9, PB_8);
00010  
00011 //Serial pc(SERIAL_TX, SERIAL_RX);
00012 //==========================
00013 bool pcf8574_write(uint8_t data,uint8_t address);
00014 bool pcf8574_read(uint8_t* data,uint8_t address);
00015 int DO_write(uint8_t value,uint8_t address);
00016 int DI_read(uint8_t address);
00017 //======================================================================
00018 ros::NodeHandle  nh;
00019 std_msgs::Int16 DO;
00020 void DO_ACT(const std_msgs::Int16 &msg){
00021    DO_write(msg.data, PCF8574_ADDR_1);
00022 }
00023 ros::Subscriber<std_msgs::Int16> ACT_sub("DO_data", &DO_ACT);
00024 
00025 std_msgs::Int16 DI;
00026 ros::Publisher DI_pub("DI_pub", &DI);
00027 //======================================================================
00028 
00029 bool pcf8574_write(uint8_t data,uint8_t address){
00030     return i2c.write(address, (char*) &data, 1, 0) == 0;
00031 }
00032 
00033 bool pcf8574_read(uint8_t* data,uint8_t address){
00034     return i2c.read(address, (char*) data, 1, 0) == 0;
00035 }
00036 
00037 int DO_write(uint8_t value,uint8_t address){
00038     int ret;
00039   
00040     ret = pcf8574_write(value,address);
00041 
00042     return ret;
00043 }
00044 int DI_read(uint8_t address){
00045     int ret;
00046     uint8_t data=0;
00047 
00048     ret = pcf8574_read(&data,address);
00049     if(!ret) return -2;
00050     
00051     return data;
00052 }
00053 
00054 
00055 int main()
00056 {
00057      nh.initNode();
00058      nh.advertise(DI_pub);
00059      nh.subscribe(ACT_sub);
00060 //    printf("Hello PCF8574\n");
00061     while (1) {
00062 //        pc.printf("Read DI=%i\r\n", DI_read(PCF8574_ADDR_2));
00063  
00064 //        DO_PC_9 = 0;
00065     
00066 //        wait(0.1);
00067         DI.data = DI_read(PCF8574_ADDR_2);
00068         DI_pub.publish( &DI);
00069         nh.spinOnce();   
00070         wait_ms(100);
00071     }
00072  
00073 }
00074