Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011 #include <ros.h>
00012 #include <std_msgs/Int32.h>
00013 #include <stdio.h>
00014 #include <unistd.h>
00015 #include "qegpioint.h"
00016
00017 ros::NodeHandle nh;
00018 std_msgs::Int32 range;
00019 ros::Publisher sonar1("sonar1", &range);
00020
00021 char *rosSrvrIp = "192.168.11.9";
00022
00023 #define USPI 150
00024 #define BIAS 300
00025
00026 unsigned long diff(struct timeval *ptv0, struct timeval *ptv1)
00027 {
00028 long val;
00029
00030 val = ptv1->tv_usec - ptv0->tv_usec;
00031 val += (ptv1->tv_sec - ptv0->tv_sec)*1000000;
00032
00033 return val;
00034 }
00035
00036 void callback(unsigned int io, struct timeval *ptv, void *userdata)
00037 {
00038 static struct timeval tv0;
00039 static int flag = 0;
00040 int sonarVal;
00041
00042 if (io==0)
00043 {
00044 flag = 1;
00045 tv0 = *ptv;
00046 }
00047
00048 if (io==1 && flag)
00049 {
00050 sonarVal = diff(&tv0, ptv);
00051 if (sonarVal>BIAS)
00052 sonarVal = (sonarVal-BIAS)/USPI;
00053 range.data = sonarVal;
00054 printf("%d\n", sonarVal);
00055 }
00056 }
00057
00058
00059
00060
00061 int main()
00062 {
00063 CQEGpioInt &gpio = CQEGpioInt::GetRef();
00064 volatile unsigned int d;
00065
00066
00067 gpio.SetData(0x0000);
00068 gpio.SetDataDirection(0x0001);
00069
00070
00071
00072 gpio.RegisterCallback(0, NULL, callback);
00073 gpio.RegisterCallback(1, NULL, callback);
00074 gpio.SetInterruptMode(0, QEG_INTERRUPT_NEGEDGE);
00075 gpio.SetInterruptMode(1, QEG_INTERRUPT_NEGEDGE);
00076
00077
00078 nh.initNode(rosSrvrIp);
00079 nh.advertise(sonar1);
00080
00081
00082 while(1)
00083 {
00084 gpio.SetData(0x0001);
00085 for (d=0; d<120000; d++);
00086 gpio.SetData(0x0000);
00087 sleep(1);
00088 sleep(1);
00089 sonar1.publish( &range );
00090 nh.spinOnce();
00091 }
00092 }
00093
00094