Diff: main.cpp

Revision:

2:47663f3fec3a

Parent:

1:7ba9803d4e01

Child:

3:ab392a9f941d

--- a/main.cpp	Mon Jul 16 19:47:46 2018 +0000
+++ b/main.cpp	Thu Jul 19 20:12:47 2018 +0000
@@ -4,46 +4,40 @@
 #include "geometry_msgs/Quaternion.h"
 
 //PINA / PINB / INDEXPIN / PPR / ENCODING (X2 BY DEFAULT)
-QEI leftEncoder(D3, D12, NC, 378);
-QEI rightEncoder(D2, D13, NC, 378);
+QEI leftX(D6, D7, NC, 378,QEI::X2_ENCODING);
+QEI leftY(D8, D9, NC, 378,QEI::X2_ENCODING);
+QEI rightX(D0, D1, NC, 378,QEI::X2_ENCODING);
+QEI rightY(D2, D3, NC, 378,QEI::X2_ENCODING);
 
 ros::NodeHandle nh;
 geometry_msgs::Quaternion wheelMsg;
 ros::Publisher wheelPub("/odom_msg", &wheelMsg); 
 
 //rosserial does not like volatile ints
-int leftPulseA=0; 
-int rightPulseA=0;
-
-//timeL; 
-//timeR; 
+int leftPosX=0; 
+int leftPosY=0;
+int rightPosX=0; 
+int rightPosY=0;
 
 int main(){
     
     nh.initNode();
     //nh.advertise(chatter);
     nh.advertise(wheelPub);
-   // pc.baud(57600);
-
+    // pc.baud(57600);
     
     while(true){
     
-    leftPulseA=leftEncoder.getPulses();
-    rightPulseA = rightEncoder.getPulses();
-    //timeL = tL.read();
-    //timeR = tL.read();
+    leftPosX = leftX.getPulses();
+    leftPosY = leftY.getPulses();
+    rightPosX = rightX.getPulses();
+    rightPosY = rightY.getPulses();
     
-    wheelMsg.x= leftPulseA;
-    wheelMsg.y = 0.0;//timeL;
-    wheelMsg.z = rightPulseA;
-    wheelMsg.w = 0.0;//timeR;    
-    
+    wheelMsg.x= leftPosX;
+    wheelMsg.y = leftPosY;
+    wheelMsg.z = rightPosX;
+    wheelMsg.w = rightPosY;    
     
-    //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);