main.cpp

00001 #include "mbed.h"
00002 #include "motordriver.h"
00003 #include "ros.h"
00004 #include "geometry_msgs/Vector3.h"
00005 #include "QEI.h"
00006 #include "geometry_msgs/Quaternion.h"
00007 #include "ros/time.h"
00008 //#include "std_msgs/String.h"
00009 
00010 ros:: NodeHandle nh; 
00011 
00012 //pwm, fwd, rev, can brake
00013 Motor motorLeft(D9,D5,D4, 1);
00014 Motor motorRight(D10, D7, D8, 1); 
00015 
00016 //PINA / PINB / INDEXPIN / PPR / ENCODING (X2 BY DEFAULT)
00017 QEI leftEncoder(D3, D12, NC, 378);
00018 QEI rightEncoder(D2, D13, NC, 378);
00019 
00020 void messageCb( const geometry_msgs::Vector3& msg)
00021 {
00022        motorLeft.speed(msg.x);
00023        motorRight.speed(msg.y);
00024 }
00025 
00026 
00027 geometry_msgs::Quaternion wheelMsg;
00028 ros::Publisher wheelPub("/odom_msg", &wheelMsg); 
00029 
00030 //std_msgs::String str_msg; 
00031 //ros::Publisher chatter("chatter", &str_msg);
00032 
00033 ros::Subscriber<geometry_msgs::Vector3> sub("/motor/command", &messageCb );
00034 
00035 int leftPulseA=0; 
00036 int rightPulseA=0;
00037 int lastLeftPulse=0;
00038 int lastRightPulse=0;
00039 unsigned long rightTime;
00040 unsigned long leftTime;
00041 unsigned long timeStamp;
00042 
00043 
00044 int main() {
00045     
00046     nh.initNode();
00047     nh.subscribe(sub);
00048     //nh.advertise(chatter);
00049     nh.advertise(wheelPub);
00050     
00051     while (true) {
00052        
00053     leftPulseA=leftEncoder.getPulses();
00054     rightPulseA = rightEncoder.getPulses();
00055     leftTime = leftEncoder.getTime();
00056     rightTime = rightEncoder.getTime();
00057     
00058     if(leftTime>rightTime)
00059         timeStamp=leftTime;
00060     else
00061         timeStamp=rightTime;
00062     
00063     wheelMsg.x= leftPulseA;
00064     wheelMsg.y = rightPulseA;
00065     wheelMsg.z = timeStamp;
00066     wheelMsg.w = 0.0;     
00067     
00068     
00069     if((lastLeftPulse!=leftPulseA)||(lastRightPulse!=rightPulseA)){
00070         wheelPub.publish(&wheelMsg);
00071         lastLeftPulse=leftPulseA;
00072         lastRightPulse=rightPulseA;
00073     }
00074     
00075     nh.spinOnce();
00076     wait_ms(250);
00077     } 
00078 }
00079