Diff: main.cpp

Revision:

0:1a95e3b1026a

Child:

1:7ba9803d4e01

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Jul 16 18:35:30 2018 +0000
@@ -0,0 +1,60 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "ros.h"
+#include "geometry_msgs/Quaternion.h"
+//#include "std_msgs/String.h"
+
+//PINA / PINB / INDEXPIN / PPR / ENCODING (X2 BY DEFAULT)
+QEI leftEncoder(D3, D12, NC, 378);
+QEI rightEncoder(D2, D13, NC, 378);
+
+Timer tL, tR; 
+
+//Serial pc(USBTX, USBRX); 
+
+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";
+
+int leftPulseA=0; 
+int rightPulseA=0;
+
+unsigned long timeL =0 ; 
+unsigned long timeR=0; 
+
+int main(){
+    
+    nh.initNode();
+    //nh.advertise(chatter);
+    nh.advertise(wheelPub);
+   // pc.baud(57600);
+
+    
+    while(true){
+    
+    leftPulseA=leftEncoder.getPulses();
+    rightPulseA = rightEncoder.getPulses();
+    timeL = tL.read();
+    timeR = tL.read();
+    
+    wheelMsg.x= leftPulseA;
+    wheelMsg.y = timeL;;
+    wheelMsg.z = rightPulseA;
+    wheelMsg.w = timeR;    
+    
+    
+    //pc.printf("Left pulses %i \n", leftPulseA);
+    //pc.printf("Right pulses %i \n", rightPulseA);
+    
+    //str_msg.data=hello;
+    //chatter.publish(&str_msg);
+    wheelPub.publish(&wheelMsg);
+    nh.spinOnce();
+    wait(0.01);
+    }
+}
\ No newline at end of file