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@10:52f59f5a9b7d, 2018-08-17 (annotated)

Committer:
sreeraj
Date:
Fri Aug 17 17:32:07 2018 +0000
Revision:
10:52f59f5a9b7d
Parent:
9:e705f028f853 
Differential Drive Method

Who changed what in which revision?

UserRevisionLine numberNew contents of line
apriljunio 0:1a95e3b1026a 1 #include "mbed.h"
apriljunio 0:1a95e3b1026a 2 #include "QEI.h"
apriljunio 0:1a95e3b1026a 3 #include "ros.h"
sreeraj 7:f71761cac14a 4 #include "ros/time.h"
sreeraj 7:f71761cac14a 5 #include <tf/tf.h>
sreeraj 8:f118cc66e271 6 #include <math.h>
sreeraj 10:52f59f5a9b7d 7
apriljunio 0:1a95e3b1026a 8 #include "geometry_msgs/Quaternion.h"
sreeraj 7:f71761cac14a 9 #include "geometry_msgs/Vector3.h"
sreeraj 9:e705f028f853 10 #include "iostream"
sreeraj 10:52f59f5a9b7d 11 #include "std_msgs/Float64.h"
sreeraj 7:f71761cac14a 12
sreeraj 7:f71761cac14a 13
sreeraj 7:f71761cac14a 14 ros::NodeHandle nh;
apriljunio 0:1a95e3b1026a 15
apriljunio 0:1a95e3b1026a 16 //PINA / PINB / INDEXPIN / PPR / ENCODING (X2 BY DEFAULT)
apriljunio 3:ab392a9f941d 17 QEI leftX(D4, D5, NC, 378,QEI::X2_ENCODING);
apriljunio 3:ab392a9f941d 18 QEI leftY(D10, D11, NC, 378,QEI::X2_ENCODING);
apriljunio 2:47663f3fec3a 19 QEI rightX(D0, D1, NC, 378,QEI::X2_ENCODING);
apriljunio 2:47663f3fec3a 20 QEI rightY(D2, D3, NC, 378,QEI::X2_ENCODING);
apriljunio 0:1a95e3b1026a 21
sreeraj 7:f71761cac14a 22
apriljunio 0:1a95e3b1026a 23 geometry_msgs::Quaternion wheelMsg;
apriljunio 0:1a95e3b1026a 24 ros::Publisher wheelPub("/odom_msg", &wheelMsg);
apriljunio 0:1a95e3b1026a 25
sreeraj 10:52f59f5a9b7d 26 std_msgs::Float64 lwheelMsg;
sreeraj 10:52f59f5a9b7d 27 ros::Publisher lwheelPub("/Lodom_msg", &lwheelMsg);
sreeraj 8:f118cc66e271 28
sreeraj 10:52f59f5a9b7d 29 std_msgs::Float64 rwheelMsg;
sreeraj 10:52f59f5a9b7d 30 ros::Publisher rwheelPub("/Rodom_msg", &rwheelMsg);
sreeraj 7:f71761cac14a 31
sreeraj 7:f71761cac14a 32
sreeraj 7:f71761cac14a 33
apriljunio 1:7ba9803d4e01 34 //rosserial does not like volatile ints
apriljunio 2:47663f3fec3a 35 int leftPosX=0;
apriljunio 2:47663f3fec3a 36 int leftPosY=0;
apriljunio 2:47663f3fec3a 37 int rightPosX=0;
apriljunio 2:47663f3fec3a 38 int rightPosY=0;
sreeraj 8:f118cc66e271 39
sreeraj 8:f118cc66e271 40
apriljunio 0:1a95e3b1026a 41 int main(){
sreeraj 6:1508faf1f383 42
apriljunio 0:1a95e3b1026a 43 nh.initNode();
apriljunio 0:1a95e3b1026a 44 nh.advertise(wheelPub);
sreeraj 10:52f59f5a9b7d 45 nh.advertise(lwheelPub);
sreeraj 10:52f59f5a9b7d 46 nh.advertise(rwheelPub);
apriljunio 0:1a95e3b1026a 47 while(true){
apriljunio 2:47663f3fec3a 48 leftPosX = leftX.getPulses();
apriljunio 2:47663f3fec3a 49 leftPosY = leftY.getPulses();
apriljunio 2:47663f3fec3a 50 rightPosX = rightX.getPulses();
apriljunio 2:47663f3fec3a 51 rightPosY = rightY.getPulses();
apriljunio 0:1a95e3b1026a 52
sreeraj 7:f71761cac14a 53 wheelMsg.x= leftPosX/10;
sreeraj 7:f71761cac14a 54 wheelMsg.y = leftPosY/10;
sreeraj 7:f71761cac14a 55 wheelMsg.z = rightPosX/10;
sreeraj 8:f118cc66e271 56 wheelMsg.w = rightPosY/10;
sreeraj 10:52f59f5a9b7d 57 lwheelMsg.data=wheelMsg.y;
sreeraj 10:52f59f5a9b7d 58 rwheelMsg.data= wheelMsg.w;
sreeraj 10:52f59f5a9b7d 59 //lwheelMsg.data= (sqrt(pow((wheelMsg.x),2)+pow((wheelMsg.y),2)));
sreeraj 10:52f59f5a9b7d 60 //rwheelMsg.data= sqrt(pow(wheelMsg.z,2)+pow(wheelMsg.w,2));
sreeraj 7:f71761cac14a 61
sreeraj 7:f71761cac14a 62
sreeraj 10:52f59f5a9b7d 63 wheelPub.publish(&wheelMsg);
sreeraj 10:52f59f5a9b7d 64 lwheelPub.publish(&lwheelMsg);
sreeraj 10:52f59f5a9b7d 65 rwheelPub.publish(&rwheelMsg);
sreeraj 9:e705f028f853 66
apriljunio 0:1a95e3b1026a 67 nh.spinOnce();
sreeraj 10:52f59f5a9b7d 68 wait(.35);
apriljunio 0:1a95e3b1026a 69 }
sreeraj 7:f71761cac14a 70 }