Diff: main.cpp

Revision:

1:b3a2c43439bd

Parent:

0:daf4d39b8c98

--- a/main.cpp	Wed Nov 05 08:01:34 2014 +0000
+++ b/main.cpp	Mon Oct 15 09:53:46 2018 +0000
@@ -1,45 +1,74 @@
 #include "mbed.h"
- 
-#define PCF8574_ADDR     (0x40) 
- 
-I2C i2c(I2C_SDA, I2C_SCL);
+#include <ros.h>
+#include <ros/time.h>
+#include <std_msgs/Int16.h>
+#define PCF8574_ADDR     (0x40)
+#define PCF8574_ADDR_1     (0x40) 
+#define PCF8574_ADDR_2     (0x42) 
+
+I2C i2c(PB_9, PB_8);
  
- 
-Serial pc(SERIAL_TX, SERIAL_RX);
+//Serial pc(SERIAL_TX, SERIAL_RX);
+//==========================
+bool pcf8574_write(uint8_t data,uint8_t address);
+bool pcf8574_read(uint8_t* data,uint8_t address);
+int DO_write(uint8_t value,uint8_t address);
+int DI_read(uint8_t address);
+//======================================================================
+ros::NodeHandle  nh;
+std_msgs::Int16 DO;
+void DO_ACT(const std_msgs::Int16 &msg){
+   DO_write(msg.data, PCF8574_ADDR_1);
+}
+ros::Subscriber<std_msgs::Int16> ACT_sub("DO_data", &DO_ACT);
 
+std_msgs::Int16 DI;
+ros::Publisher DI_pub("DI_pub", &DI);
+//======================================================================
 
-bool pcf8574_write(uint8_t data){
-    return i2c.write(PCF8574_ADDR, (char*) &data, 1, 0) == 0;
+bool pcf8574_write(uint8_t data,uint8_t address){
+    return i2c.write(address, (char*) &data, 1, 0) == 0;
 }
 
-bool pcf8574_read(uint8_t* data){
-    return i2c.read(PCF8574_ADDR, (char*) data, 1, 0) == 0;
+bool pcf8574_read(uint8_t* data,uint8_t address){
+    return i2c.read(address, (char*) data, 1, 0) == 0;
 }
 
-int pcf8574_test(uint8_t value){
+int DO_write(uint8_t value,uint8_t address){
+    int ret;
+  
+    ret = pcf8574_write(value,address);
+
+    return ret;
+}
+int DI_read(uint8_t address){
     int ret;
     uint8_t data=0;
-    
-    ret = pcf8574_write(value);
-    if(!ret) return -1;
-    
-    ret = pcf8574_read(&data);
+
+    ret = pcf8574_read(&data,address);
     if(!ret) return -2;
     
     return data;
 }
 
+
 int main()
 {
- 
-    printf("Hello PCF8574\n");
+     nh.initNode();
+     nh.advertise(DI_pub);
+     nh.subscribe(ACT_sub);
+//    printf("Hello PCF8574\n");
     while (1) {
-        pc.printf("pcf8574_test: low: %i\n", pcf8574_test(0x00));
-        wait(0.5);
-        pc.printf("pcf8574_test: high: %i\n", pcf8574_test(0xff));
-        wait(0.5);
+//        pc.printf("Read DI=%i\r\n", DI_read(PCF8574_ADDR_2));
+ 
+//        DO_PC_9 = 0;
+    
+//        wait(0.1);
+        DI.data = DI_read(PCF8574_ADDR_2);
+        DI_pub.publish( &DI);
+        nh.spinOnce();   
+        wait_ms(100);
     }
  
 }
- 
- 
+ 
\ No newline at end of file