added broadcaster for l.mouse
Dependencies: QEI mbed ros_lib_kinetic
Fork of Mouse_ISR_broadcaster by
main.cpp
- Committer:
- apriljunio
- Date:
- 2018-07-16
- Revision:
- 0:1a95e3b1026a
- Child:
- 1:7ba9803d4e01
File content as of revision 0:1a95e3b1026a:
#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); } }