Diff: main.cpp

Revision:

2:82249618b6bc

Parent:

1:5f2cb6bc0a8e

Child:

3:9202fa788d3c

--- a/main.cpp	Fri Jul 13 20:35:51 2018 +0000
+++ b/main.cpp	Mon Jul 16 19:46:36 2018 +0000
@@ -2,33 +2,62 @@
 #include "motordriver.h"
 #include "ros.h"
 #include "geometry_msgs/Vector3.h"
+#include "QEI.h"
+//#include "geometry_msgs/Quaternion.h"
+#include "std_msgs/String.h"
 
 //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";
 
 void messageCb( const geometry_msgs::Vector3& msg)
 {
-      //while (true) {
-      //for (float s=-msg.x; s < msg.x ; s += 0.01f) {
        motorLeft.speed(msg.x);
        motorRight.speed(msg.y);
-      // wait(0.005);}
-   // }
 }
 
 ros::Subscriber<geometry_msgs::Vector3> sub("/motor/command", &messageCb );
 
+//int leftPulseA=0; 
+//int rightPulseA=0;
+
 int main() {
     
     nh.initNode();
     nh.subscribe(sub);
+    nh.advertise(chatter);
+    //nh.advertise(wheelPub);
     
     while (true) {
-        nh.spinOnce();
-        wait_ms(1);
+       
+//    leftPulseA=leftEncoder.getPulses();
+//    rightPulseA = rightEncoder.getPulses();
+    /*
+    wheelMsg.x= leftPulseA;
+    wheelMsg.y = rightPulseA;
+    wheelMsg.z = 0.0;
+    wheelMsg.w = 0.0;    
+    */
+    
+    //wheelPub.publish(&wheelMsg);
+    str_msg.data=hello;
+    chatter.publish(&str_msg);
+    nh.spinOnce();
+    wait_ms(1);
     } 
 }