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-20
Revision:
3:ab392a9f941d
Parent:
2:47663f3fec3a
Child:
4:f87a415beec4
Child:
6:1508faf1f383

File content as of revision 3:ab392a9f941d:

#include "mbed.h"
#include "QEI.h"
#include "ros.h"
#include "geometry_msgs/Quaternion.h"

//PINA / PINB / INDEXPIN / PPR / ENCODING (X2 BY DEFAULT)
QEI leftX(D4, D5, NC, 378,QEI::X2_ENCODING);
QEI leftY(D10, D11, 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 leftPosX=0; 
int leftPosY=0;
int rightPosX=0; 
int rightPosY=0;

int main(){
    
    nh.initNode();
    //nh.advertise(chatter);
    nh.advertise(wheelPub);
    // pc.baud(57600);
    
    while(true){
    
    leftPosX = leftX.getPulses();
    leftPosY = leftY.getPulses();
    rightPosX = rightX.getPulses();
    rightPosY = rightY.getPulses();
    
    wheelMsg.x= leftPosX;
    wheelMsg.y = leftPosY;
    wheelMsg.z = rightPosX;
    wheelMsg.w = rightPosY;    
    
    wheelPub.publish(&wheelMsg);
    nh.spinOnce();
    wait(0.01);
    }
}