VEXProRangeMotorLoop.cpp
Go to the documentation of this file.
00001 /*
00002  * VEXProRangeMotorLoop.cpp
00003  *
00004  *  Created on: Jul 13, 2012
00005  *      Author: bouchier
00006  *  Publish the range from a Sonar connected to digital 1 (Input) &
00007  *  digital 2 (Output) on topic sonar1. Control motor 13 speed by
00008  *  publishing the desired speed on a ros topic with e.g.
00009  * $ rostopic pub my_topic std_msgs/Int32 120.
00010  * Drives the motor on VEXPro motor13 connection to the requested value: -255 to +255
00011  *  that is received on subscribed topic motor1
00012  *
00013  * Note: connector labeled "INPUT" on sonar sensor goes to
00014  * digital 1 (bit 0), and connector labeled "OUTPUT" goes to
00015  * digital 2 (bit 1).
00016  */
00017 
00018 
00019 #include <ros.h>
00020 #include <std_msgs/Int32.h>
00021 #include <stdio.h>
00022 #include <unistd.h>
00023 #include <qegpioint.h>
00024 #include <qemotoruser.h>
00025 
00026 ros::NodeHandle  nh;
00027 std_msgs::Int32 range;
00028 ros::Publisher sonar1("sonar1", &range);
00029 CQEMotorUser &motor = CQEMotorUser::GetRef();   // motor singleton
00030 CQEGpioInt &gpio = CQEGpioInt::GetRef();                // GPIO singleton
00031 
00032 char *rosSrvrIp = "192.168.11.9";
00033 
00034 #define USPI 150
00035 #define BIAS 300
00036 
00037 /*
00038  * Motor callback - called when new motor speed is published
00039  */
00040 void motorCb(const std_msgs::Int32& motor13_msg){
00041         int speed = motor13_msg.data;
00042         printf("Received subscribed motor speed %d\n", speed);
00043     motor.SetPWM(0, speed);
00044 }
00045 ros::Subscriber<std_msgs::Int32> motorSub("motor13", motorCb );
00046 
00047 /*
00048  * Calculate difference in usec between sonar start & end timevals
00049  */
00050 unsigned long diff(struct timeval *ptv0, struct timeval *ptv1)
00051 {
00052         long val;
00053 
00054         val = ptv1->tv_usec - ptv0->tv_usec;
00055         val += (ptv1->tv_sec - ptv0->tv_sec)*1000000;
00056 
00057         return val;
00058 }
00059 
00060 /*
00061  * Sonar callback. Called at interrupt level when sonar output transitions,
00062  * indicating end of range measurement
00063  */
00064 void callback(unsigned int io, struct timeval *ptv, void *userdata)
00065 {
00066         static struct timeval tv0;
00067         static int flag = 0;
00068         int sonarVal;
00069 
00070         if (io==0)
00071         {
00072                 flag = 1;
00073                 tv0 = *ptv;
00074         }
00075 
00076         if (io==1 && flag)
00077         {
00078                 sonarVal = diff(&tv0, ptv);
00079                 if (sonarVal>BIAS)
00080                         sonarVal = (sonarVal-BIAS)/USPI;
00081                 range.data = sonarVal;
00082                 //printf("%d\n", sonarVal);
00083         }
00084 }
00085 
00086 int main()
00087 {
00088         volatile unsigned int d;
00089 
00090         /* initialize ROS & subscribers & publishers */
00091         //nh.initNode();
00092         nh.initNode(rosSrvrIp);
00093         nh.advertise(sonar1);   // advertise sonar range topic
00094         nh.subscribe(motorSub);         // subscribe to motor speed topic
00095 
00096         // reset bit 0, set as output for sonar trigger
00097         gpio.SetData(0x0000);
00098         gpio.SetDataDirection(0x0001);
00099 
00100         // set callbacks on negative edge for both bits 0 (trigger)
00101         // and 1 (echo)
00102         gpio.RegisterCallback(0, NULL, callback);
00103         gpio.RegisterCallback(1, NULL, callback);
00104         gpio.SetInterruptMode(0, QEG_INTERRUPT_NEGEDGE);
00105         gpio.SetInterruptMode(1, QEG_INTERRUPT_NEGEDGE);
00106 
00107         // trigger sonar by toggling bit 0
00108         while(1)
00109         {
00110                 gpio.SetData(0x0001);
00111                 for (d=0; d<120000; d++);
00112                 gpio.SetData(0x0000);
00113                 usleep(100000);         // the interrupt breaks us out of this sleep
00114                 usleep(100000);         // now really sleep
00115                 sonar1.publish( &range );
00116                 nh.spinOnce();
00117         }
00118 }
00119 
00120 


rosserial_embeddedlinux
Author(s): Paul Bouchier
autogenerated on Fri Dec 6 2013 20:35:58