SBD Digital Accelerator Robotics / Mbed 2 deprecated Mouse_ISR_test1

added broadcaster for l.mouse

Dependencies:   QEI mbed ros_lib_kinetic

Fork of Mouse_ISR_broadcaster by SBD Digital Accelerator Robotics

main.cpp

Committer:
apriljunio
Date:
2018-07-16
Revision:
1:7ba9803d4e01
Parent:
0:1a95e3b1026a
Child:
2:47663f3fec3a

File content as of revision 1:7ba9803d4e01:

#include "mbed.h"
#include "QEI.h"
#include "ros.h"
#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);

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 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 = 0.0;//timeL;
    wheelMsg.z = rightPulseA;
    wheelMsg.w = 0.0;//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);
    }
}