Diff: main.cpp

Revision:

8:4974fc24fbd7

Parent:

7:1a234f02746f

Child:

9:f9e9662d26bf

diff -r 1a234f02746f -r 4974fc24fbd7 main.cpp
--- a/main.cpp	Thu May 10 08:16:14 2018 +0000
+++ b/main.cpp	Tue May 15 00:46:30 2018 +0000
@@ -9,7 +9,7 @@
 
 #include <ros.h>
 #include <ros/time.h>
-#include <std_msgs/Empty.h>
+#include <std_msgs/Int16.h>
 #include <std_msgs/String.h>
 #include <std_msgs/Float32.h>
 #include <sensor_msgs/BatteryState.h>
@@ -46,6 +46,10 @@
 DigitalOut          Receiver(D7);                     //RS485_E
 DigitalOut          CAN_T(D14);
 DigitalOut          CAN_R(D15);
+DigitalOut          DO_0(PC_8);
+DigitalOut          DO_1(PC_9);
+DigitalIn           DI_0(PB_13);
+
 //CAN                 can1(PB_8,PB_9);  // CAN Rx pin name, CAN Tx pin name
 //CANMsg              rxMsg;
 //CANMessage          rxMsg;
@@ -53,17 +57,52 @@
 
 MPU6050 mpu;//(PB_7,PB_6);     // sda, scl pin
 
+ros::NodeHandle  nh;
+//======================================================================
 tiny_msgs::tinyIMU imu_msg;
-std_msgs::String str_msg;
+ros::Publisher imu_pub("tinyImu", &imu_msg);
+//======================================================================
+
+//======================================================================
 std_msgs::Float32 VelAngular_L;
+ros::Publisher pub_lmotor("pub_lmotor", &VelAngular_L);
+//======================================================================
+
+//======================================================================
 std_msgs::Float32 VelAngular_R;
+ros::Publisher pub_rmotor("pub_rmotor", &VelAngular_R);
+//======================================================================
+
+//======================================================================
 sensor_msgs::BatteryState BTState;
+ros::Publisher BT_pub("BatteryState", &BTState);
+//======================================================================
 
-ros::NodeHandle  nh;
-ros::Publisher imu_pub("tinyImu", &imu_msg);
-ros::Publisher pub_lmotor("pub_lmotor", &VelAngular_L);
-ros::Publisher pub_rmotor("pub_rmotor", &VelAngular_R);
-ros::Publisher BT_pub("BatteryState", &BTState);
+//======================================================================
+std_msgs::Int16 DI;
+ros::Publisher DI_pub("DI_pub", &DI);
+//======================================================================
+
+//======================================================================
+std_msgs::Int16 DO;
+
+void DO_ACT(const std_msgs::Int16 &msg){
+    if (msg.data & 0x01){
+        DO_0 = 1;
+    }
+    else{
+        DO_0 = 0;
+    }
+
+    if (msg.data & 0x02){
+        DO_1 = 1;
+    }
+    else{
+        DO_1 = 0;
+    }    
+}
+ros::Subscriber<std_msgs::Int16> ACT_sub("DO_data", &DO_ACT);
+//======================================================================
 uint32_t seq;
 
 #define IMU_FIFO_RATE_DIVIDER 0x09
@@ -87,15 +126,17 @@
 uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
 int16_t ax, ay, az;
 int16_t gx, gy, gz;
-  float Lrpm,Rrpm;
-    float ticks_since_target;
-    double timeout_ticks;
+float Lrpm,Rrpm;
+float ticks_since_target;
+double timeout_ticks;
 
     double w;
     double rate;
     double Dimeter;
     float dx,dy,dr;
-        
+int lastButtonState = 1;    
+int buttonState;     
+int db_conter = 0;
 int buffer[9] = {0};
 int dataH,datanum;
 //=========RS485
@@ -387,13 +428,19 @@
 }
 //=======================================================
 bool Init() {
+    DO_0 = 0;
+    DO_1 = 0;
+    
     seq = 0;
     nh.initNode();
     nh.advertise(imu_pub);
     nh.advertise(pub_lmotor);
     nh.advertise(pub_rmotor);
     nh.advertise(BT_pub);
+    nh.advertise(DI_pub);
     nh.subscribe(cmd_vel_sub);
+    nh.subscribe(ACT_sub);
+    
     mpu.initialize();
     if (mpu.testConnection()) {
 //        pc.printf("MPU6050 test connection passed.\n");
@@ -428,6 +475,7 @@
     RS232.baud(PC_BAUDRATE);
     MBED_ASSERT(Init() == true);
     CANMessage rxMsg;
+    DI_0.mode(PullUp);
     CAN_T = 0;
     CAN_R = 0;
     wait_ms(50);
@@ -451,6 +499,26 @@
         imu_msg.gyro.z = gz;   
 //        
         imu_pub.publish( &imu_msg );
+        //============DI==================
+        int reading = DI_0;
+        if (reading == lastButtonState) {
+            db_conter = db_conter+1;
+        }
+        else{
+            db_conter = 0;
+        }
+        
+        if (db_conter > 3) {
+//            if (reading != buttonState) {
+            buttonState = reading;
+            DI.data = buttonState; 
+//            }
+        }
+
+        lastButtonState = reading;
+        DI_pub.publish( &DI);
+
+        //=========================================
         if (can1.read(rxMsg) && (t.read_ms() > 5000))  {
 //            RS232.printf("  ID      = 0x%.3x\r\n", rxMsg.id);