SBD Digital Accelerator Robotics / Mbed 2 deprecated Test

VNH ROSSERIAL PUB+SUB

Dependencies:   Motordriver QEI mbed ros_lib_kinetic

Fork of Test by SBD Digital Accelerator Robotics

main.cpp@5:586f5122b731, 2018-07-20 (annotated)

Committer:
apriljunio
Date:
Fri Jul 20 16:15:12 2018 +0000
Revision:
5:586f5122b731
Parent:
4:55882b6d5d9a 
Newest Version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
apriljunio 0:99538487421b 1 #include "mbed.h"
apriljunio 0:99538487421b 2 #include "motordriver.h"
apriljunio 0:99538487421b 3 #include "ros.h"
apriljunio 0:99538487421b 4 #include "geometry_msgs/Vector3.h"
apriljunio 2:82249618b6bc 5 #include "QEI.h"
apriljunio 3:9202fa788d3c 6 #include "geometry_msgs/Quaternion.h"
apriljunio 3:9202fa788d3c 7 #include "ros/time.h"
apriljunio 3:9202fa788d3c 8 //#include "std_msgs/String.h"
apriljunio 3:9202fa788d3c 9
apriljunio 3:9202fa788d3c 10 ros:: NodeHandle nh;
apriljunio 0:99538487421b 11
apriljunio 0:99538487421b 12 //pwm, fwd, rev, can brake
apriljunio 0:99538487421b 13 Motor motorLeft(D9,D5,D4, 1);
apriljunio 0:99538487421b 14 Motor motorRight(D10, D7, D8, 1);
apriljunio 0:99538487421b 15
apriljunio 2:82249618b6bc 16 //PINA / PINB / INDEXPIN / PPR / ENCODING (X2 BY DEFAULT)
apriljunio 3:9202fa788d3c 17 QEI leftEncoder(D3, D12, NC, 378);
apriljunio 3:9202fa788d3c 18 QEI rightEncoder(D2, D13, NC, 378);
apriljunio 0:99538487421b 19
apriljunio 0:99538487421b 20 void messageCb( const geometry_msgs::Vector3& msg)
apriljunio 0:99538487421b 21 {
apriljunio 1:5f2cb6bc0a8e 22 motorLeft.speed(msg.x);
apriljunio 1:5f2cb6bc0a8e 23 motorRight.speed(msg.y);
apriljunio 0:99538487421b 24 }
apriljunio 0:99538487421b 25
apriljunio 3:9202fa788d3c 26
apriljunio 3:9202fa788d3c 27 geometry_msgs::Quaternion wheelMsg;
apriljunio 3:9202fa788d3c 28 ros::Publisher wheelPub("/odom_msg", &wheelMsg);
apriljunio 3:9202fa788d3c 29
apriljunio 3:9202fa788d3c 30 //std_msgs::String str_msg;
apriljunio 3:9202fa788d3c 31 //ros::Publisher chatter("chatter", &str_msg);
apriljunio 3:9202fa788d3c 32
apriljunio 0:99538487421b 33 ros::Subscriber<geometry_msgs::Vector3> sub("/motor/command", &messageCb );
apriljunio 0:99538487421b 34
apriljunio 3:9202fa788d3c 35 int leftPulseA=0;
apriljunio 3:9202fa788d3c 36 int rightPulseA=0;
apriljunio 4:55882b6d5d9a 37 int lastLeftPulse=0;
apriljunio 4:55882b6d5d9a 38 int lastRightPulse=0;
apriljunio 3:9202fa788d3c 39 unsigned long rightTime;
apriljunio 3:9202fa788d3c 40 unsigned long leftTime;
apriljunio 4:55882b6d5d9a 41 unsigned long timeStamp;
apriljunio 3:9202fa788d3c 42
apriljunio 2:82249618b6bc 43
apriljunio 0:99538487421b 44 int main() {
apriljunio 0:99538487421b 45
apriljunio 0:99538487421b 46 nh.initNode();
apriljunio 0:99538487421b 47 nh.subscribe(sub);
apriljunio 3:9202fa788d3c 48 //nh.advertise(chatter);
apriljunio 3:9202fa788d3c 49 nh.advertise(wheelPub);
apriljunio 0:99538487421b 50
apriljunio 0:99538487421b 51 while (true) {
apriljunio 2:82249618b6bc 52
apriljunio 3:9202fa788d3c 53 leftPulseA=leftEncoder.getPulses();
apriljunio 3:9202fa788d3c 54 rightPulseA = rightEncoder.getPulses();
apriljunio 3:9202fa788d3c 55 leftTime = leftEncoder.getTime();
apriljunio 3:9202fa788d3c 56 rightTime = rightEncoder.getTime();
apriljunio 3:9202fa788d3c 57
apriljunio 4:55882b6d5d9a 58 if(leftTime>rightTime)
apriljunio 4:55882b6d5d9a 59 timeStamp=leftTime;
apriljunio 4:55882b6d5d9a 60 else
apriljunio 4:55882b6d5d9a 61 timeStamp=rightTime;
apriljunio 4:55882b6d5d9a 62
apriljunio 2:82249618b6bc 63 wheelMsg.x= leftPulseA;
apriljunio 2:82249618b6bc 64 wheelMsg.y = rightPulseA;
apriljunio 4:55882b6d5d9a 65 wheelMsg.z = timeStamp;
apriljunio 4:55882b6d5d9a 66 wheelMsg.w = 0.0;
apriljunio 4:55882b6d5d9a 67
apriljunio 3:9202fa788d3c 68
apriljunio 4:55882b6d5d9a 69 if((lastLeftPulse!=leftPulseA)||(lastRightPulse!=rightPulseA)){
apriljunio 3:9202fa788d3c 70 wheelPub.publish(&wheelMsg);
apriljunio 4:55882b6d5d9a 71 lastLeftPulse=leftPulseA;
apriljunio 4:55882b6d5d9a 72 lastRightPulse=rightPulseA;
apriljunio 3:9202fa788d3c 73 }
apriljunio 3:9202fa788d3c 74
apriljunio 2:82249618b6bc 75 nh.spinOnce();
apriljunio 5:586f5122b731 76 wait_ms(250);
apriljunio 0:99538487421b 77 }
apriljunio 0:99538487421b 78 }
apriljunio 0:99538487421b 79