Diff: main.cpp

Revision:

3:9202fa788d3c

Parent:

2:82249618b6bc

Child:

4:55882b6d5d9a

--- a/main.cpp	Mon Jul 16 19:46:36 2018 +0000
+++ b/main.cpp	Tue Jul 17 15:18:55 2018 +0000
@@ -3,26 +3,19 @@
 #include "ros.h"
 #include "geometry_msgs/Vector3.h"
 #include "QEI.h"
-//#include "geometry_msgs/Quaternion.h"
-#include "std_msgs/String.h"
+#include "geometry_msgs/Quaternion.h"
+#include "ros/time.h"
+//#include "std_msgs/String.h"
+
+ros:: NodeHandle nh; 
 
 //pwm, fwd, rev, can brake
 Motor motorLeft(D9,D5,D4, 1);
 Motor motorRight(D10, D7, D8, 1); 
 
 //PINA / PINB / INDEXPIN / PPR / ENCODING (X2 BY DEFAULT)
-//QEI leftEncoder(D3, D12, NC, 378);
-//QEI rightEncoder(D2, D13, NC, 378);
-
-
-ros:: NodeHandle nh; 
-//geometry_msgs::Quaternion wheelMsg;
-//ros::Publisher wheelPub("/odom_msg", &wheelMsg); 
-
-std_msgs::String str_msg; 
-ros::Publisher chatter("chatter", &str_msg);
-
-char hello[13]="Hello World";
+QEI leftEncoder(D3, D12, NC, 378);
+QEI rightEncoder(D2, D13, NC, 378);
 
 void messageCb( const geometry_msgs::Vector3& msg)
 {
@@ -30,34 +23,53 @@
        motorRight.speed(msg.y);
 }
 
+
+geometry_msgs::Quaternion wheelMsg;
+ros::Publisher wheelPub("/odom_msg", &wheelMsg); 
+
+//std_msgs::String str_msg; 
+//ros::Publisher chatter("chatter", &str_msg);
+
 ros::Subscriber<geometry_msgs::Vector3> sub("/motor/command", &messageCb );
 
-//int leftPulseA=0; 
-//int rightPulseA=0;
+int leftPulseA=0; 
+int rightPulseA=0;
+bool changeFlagR=false;
+bool changeFlagL=false;
+unsigned long rightTime;
+unsigned long leftTime;
+
 
 int main() {
     
     nh.initNode();
     nh.subscribe(sub);
-    nh.advertise(chatter);
-    //nh.advertise(wheelPub);
+    //nh.advertise(chatter);
+    nh.advertise(wheelPub);
     
     while (true) {
        
-//    leftPulseA=leftEncoder.getPulses();
-//    rightPulseA = rightEncoder.getPulses();
-    /*
+    leftPulseA=leftEncoder.getPulses();
+    rightPulseA = rightEncoder.getPulses();
+    leftTime = leftEncoder.getTime();
+    rightTime = rightEncoder.getTime();
+    
     wheelMsg.x= leftPulseA;
     wheelMsg.y = rightPulseA;
-    wheelMsg.z = 0.0;
-    wheelMsg.w = 0.0;    
-    */
+    wheelMsg.z = leftTime;
+    wheelMsg.w = rightTime;     
+    
+    changeFlagL=leftEncoder.getChange();
+    changeFlagR=rightEncoder.getChange();
     
-    //wheelPub.publish(&wheelMsg);
-    str_msg.data=hello;
-    chatter.publish(&str_msg);
+    if((changeFlagL==true)|(changeFlagR==true)){
+        wheelPub.publish(&wheelMsg);
+        changeFlagL=false;
+        changeFlagR=false;
+    }
+    
     nh.spinOnce();
-    wait_ms(1);
+    wait_ms(1000);
     } 
 }