added broadcaster for l.mouse
Dependencies: QEI mbed ros_lib_kinetic
Fork of Mouse_ISR_broadcaster by
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);