VEXProRangePublish.cpp
Go to the documentation of this file.
00001 /*
00002  * VEXProRangePublish.cpp
00003  *
00004  *  Created on: Jul 12, 2012
00005  *      Author: bouchier
00006  *
00007  *  Publish the range from an ultrasonic ranging sensor connected to digital 1 (Input) &
00008  *  digital 2 (Output) on topic sonar1
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 // Note: connector labeled "INPUT" on sonar sensor goes to
00059 // digital 1 (bit 0), and connector labeled "OUTPUT" goes to
00060 // digital 2 (bit 1).
00061 int main()
00062 {
00063   CQEGpioInt &gpio = CQEGpioInt::GetRef();
00064   volatile unsigned int d;
00065 
00066   // reset bit 0, set as output for sonar trigger
00067   gpio.SetData(0x0000);
00068   gpio.SetDataDirection(0x0001);
00069 
00070   // set callbacks on negative edge for both bits 0 (trigger)
00071   // and 1 (echo)
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         //nh.initNode();
00078         nh.initNode(rosSrvrIp);
00079         nh.advertise(sonar1);
00080 
00081   // trigger sonar by toggling bit 0
00082   while(1)
00083     {
00084       gpio.SetData(0x0001);
00085       for (d=0; d<120000; d++);
00086       gpio.SetData(0x0000);
00087       sleep(1);         // the interrupt breaks us out of this sleep
00088       sleep(1);         // now really sleep a second
00089           sonar1.publish( &range );
00090           nh.spinOnce();
00091     }
00092 }
00093 
00094 


rosserial_embeddedlinux
Author(s): Paul Bouchier
autogenerated on Fri Aug 28 2015 12:44:53