(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

VEXPro range motor control loop

Description: This tutorial describes how to use interrupts to accurately measure range using an ultrasonic rangefinder, then goes on to apply that in a motor control system with the loop controller running in a ROS node.

Tutorial Level: INTERMEDIATE

This tutorial takes you step by step from measuring range and controlling a motor through an integrated closed-loop control system for maintaining an object at a constant distance. No steering logic is introduced, so it is only useful in one dimension.

Four programs are presented in this tutorial, and the code for three are explained:

  1. VEXProRangePublish: a program which uses interrupts to accurately measure the range of an object using an ultrasonic rangefinder, and publishes the range. This program is explained in depth.
  2. VEXProMotor13Subscribe: a program which subscribes to a motor speed topic and drives the VEXPro 2-wire motor output to the requested speed.
  3. VEXProRangeMotorLoop: a program which combines the first two into a system that publishes the range and subscribes to motor speed requests published by the fourth program. The differences between this and VEXProRangePublish are explained in depth, showing how motor control is integrated with rangefinding.
  4. VEXProRangeMotorCtrlr: a ROS node written in C++ which attempts to keep an object at a constant distance by running a motor forward or backward as the object becomes nearer or further away than the setpoint. With appropriate mechanics it can be the controller part of a closed-loop control system. This program is explained in depth.

The code

Listings for three programs are presented here, along with explanations of their operation. If you understand the first: VEXProRangePublish, you'll easily see how motor control is integrated into the second: VEXProRangeMotorLoop. The third is the C++ code for the ROS node which does the closed-loop control. See "The code explained" section for line-by-line explanations of each of these programs. The testing section describes how to run all four programs and test them individually.

The code for all four programs is in the examples directory and you should be able to open them in your Terk IDE.

VEXProRangePublish

This program simply publishes the range to an obstacle sensed by an ultrasonic rangefinder.

   1 /*
   2  * VEXProRangePublish.cpp
   3  *
   4  *  Created on: Jul 12, 2012
   5  *      Author: bouchier
   6  *
   7  *  Publish the range from an ultrasonic ranging sensor connected to digital 1 (Input) &
   8  *  digital 2 (Output) on topic sonar1
   9  */
  10 
  11 #include <ros.h>
  12 #include <std_msgs/Int32.h>
  13 #include <stdio.h>
  14 #include <unistd.h>
  15 #include "qegpioint.h"
  16 
  17 ros::NodeHandle  nh;
  18 std_msgs::Int32 range;
  19 ros::Publisher sonar1("sonar1", &range);
  20 
  21 char *rosSrvrIp = "192.168.11.9";
  22 
  23 #define USPI 150
  24 #define BIAS 300
  25 
  26 unsigned long diff(struct timeval *ptv0, struct timeval *ptv1)
  27 {
  28   long val;
  29 
  30   val = ptv1->tv_usec - ptv0->tv_usec;
  31   val += (ptv1->tv_sec - ptv0->tv_sec)*1000000;
  32 
  33   return val;
  34 }
  35 
  36 void callback(unsigned int io, struct timeval *ptv, void *userdata)
  37 {
  38   static struct timeval tv0;
  39   static int flag = 0;
  40   int sonarVal;
  41 
  42    if (io==0)
  43      {
  44        flag = 1;
  45        tv0 = *ptv;
  46      }
  47 
  48   if (io==1 && flag)
  49     {
  50           sonarVal = diff(&tv0, ptv);
  51           if (sonarVal>BIAS)
  52                   sonarVal = (sonarVal-BIAS)/USPI;
  53           range.data = sonarVal;
  54       printf("%d\n", sonarVal);
  55     }
  56 }
  57 
  58 // Note: connector labeled "INPUT" on sonar sensor goes to
  59 // digital 1 (bit 0), and connector labeled "OUTPUT" goes to
  60 // digital 2 (bit 1).
  61 int main()
  62 {
  63   CQEGpioInt &gpio = CQEGpioInt::GetRef();
  64   volatile unsigned int d;
  65 
  66   // reset bit 0, set as output for sonar trigger
  67   gpio.SetData(0x0000);
  68   gpio.SetDataDirection(0x0001);
  69 
  70   // set callbacks on negative edge for both bits 0 (trigger)
  71   // and 1 (echo)
  72   gpio.RegisterCallback(0, NULL, callback);
  73   gpio.RegisterCallback(1, NULL, callback);
  74   gpio.SetInterruptMode(0, QEG_INTERRUPT_NEGEDGE);
  75   gpio.SetInterruptMode(1, QEG_INTERRUPT_NEGEDGE);
  76 
  77         //nh.initNode();
  78         nh.initNode(rosSrvrIp);
  79         nh.advertise(sonar1);
  80 
  81   // trigger sonar by toggling bit 0
  82   while(1)
  83     {
  84       gpio.SetData(0x0001);
  85       for (d=0; d<120000; d++);
  86       gpio.SetData(0x0000);
  87       sleep(1);         // the interrupt breaks us out of this sleep
  88       sleep(1);         // now really sleep a second
  89           sonar1.publish( &range );
  90           nh.spinOnce();
  91     }
  92 }

The code explained

Now let's break the code down. Here's how VEXProRangePublish works.

  11 #include <ros.h>
  12 #include <std_msgs/Int32.h>
  13 #include <stdio.h>
  14 #include <unistd.h>
  15 #include "qegpioint.h"
  16 

In addition to the std_msgs/Int32.h message used to publish the range, you include the libqwerk definition of the GPIO with Interrupts library: qegpioint.h. This gives control over the digital I/O pins on the VEXPro, and enables assigning callbacks to their interrupt.

The declaration of the publisher on the sonar1 topic at line 19 is similar to the Hello ROS example publisher.

  26 unsigned long diff(struct timeval *ptv0, struct timeval *ptv1)
  27 {
  28   long val;
  29 
  30   val = ptv1->tv_usec - ptv0->tv_usec;
  31   val += (ptv1->tv_sec - ptv0->tv_sec)*1000000;
  32 
  33   return val;
  34 }

The diff() function takes two linux timeval structs and subtracts them. Timeval presents time in two values representing seconds and microseconds. The time difference is return in microseconds. This function is used to calculate the time difference between the emitted and returning ultrasonic pulse.

  36 void callback(unsigned int io, struct timeval *ptv, void *userdata)
  37 {
  38   static struct timeval tv0;
  39   static int flag = 0;
  40   int sonarVal;
  41 
  42    if (io==0)
  43      {
  44        flag = 1;
  45        tv0 = *ptv;
  46      }
  47 
  48   if (io==1 && flag)
  49     {
  50           sonarVal = diff(&tv0, ptv);
  51           if (sonarVal>BIAS)
  52                   sonarVal = (sonarVal-BIAS)/USPI;
  53           range.data = sonarVal;
  54       printf("%d\n", sonarVal);
  55     }
  56 }

The callback() function is called at interrupt level on the negative edge of the output and input pins of the rangefinder. This happens first when the code in main() drives the output negative (the start of the acoustic pulse), and a second time when the echo is received. On the first call, it stores the current time in tv0. On the second call, it subtracts tv0 from the current time, divides it by twice the speed of sound in microseconds per inch (USPI), with the result being inches to the reflective surface. It prints the value and stores it in the data part of the range message, which will later be published by main();

  63   CQEGpioInt &gpio = CQEGpioInt::GetRef();

Here you declare the GPIO object so you can use its methods to operate on the digital pins of the VEXPro controller.

  66   // reset bit 0, set as output for sonar trigger
  67   gpio.SetData(0x0000);
  68   gpio.SetDataDirection(0x0001);
  69 
  70   // set callbacks on negative edge for both bits 0 (trigger)
  71   // and 1 (echo)
  72   gpio.RegisterCallback(0, NULL, callback);
  73   gpio.RegisterCallback(1, NULL, callback);
  74   gpio.SetInterruptMode(0, QEG_INTERRUPT_NEGEDGE);
  75   gpio.SetInterruptMode(1, QEG_INTERRUPT_NEGEDGE);

main() sets up the digital I/O pins so that digital port 1 (which corresponds to bit 0) is an output, and both are low. It then registers callback() to be called when either digital port 1 or 2 (bits 0 or 1) transition low. These trigger the timing measurement in callback() described above.

The node initialization and publisher advertisement on lines 78 - 79 work as described in the Hello ROS tutorial.

  82   while(1)
  83     {
  84       gpio.SetData(0x0001);
  85       for (d=0; d<120000; d++);
  86       gpio.SetData(0x0000);
  87       sleep(1);         // the interrupt breaks us out of this sleep
  88       sleep(1);         // now really sleep a second
  89           sonar1.publish( &range );
  90           nh.spinOnce();
  91     }

The program then loops forever in the while(1) loop, only breaking out when you terminate execution, e.g. with ^C or by stopping it with the IDE. First it sets digital pin 1 high, spins a while while the rangefinder sets up, then sets the pin low, which triggers the rangefinder to emit the ultrasonic pulse, and also triggers the first time measurement by callback(). It then sleeps. The interrupt that happens when the reflected ultrasonic pulse returns and digital pin 2 (the input) goes low triggers the second time measurement and range calculation by callback(), but also breaks main() out of its sleep. The second sleep() statement on line 88 ensures the program actually sleeps for a second between range measurements. When the sleep ends, the program publishes the range, and calls spinOnce() which sends the message to ROS (and checks for any other work that needs doing).

VEXProRangeMotorLoop

This program integrates motor operation into the rangefinder program above.

   1 /*
   2  * VEXProRangeMotorLoop.cpp
   3  *
   4  *  Created on: Jul 13, 2012
   5  *      Author: bouchier
   6  *  Publish the range from a Sonar connected to digital 1 (Input) &
   7  *  digital 2 (Output) on topic sonar1. Control motor 13 speed by
   8  *  publishing the desired speed on a ros topic with e.g.
   9  * $ rostopic pub my_topic std_msgs/Int32 120.
  10  * Drives the motor on VEXPro motor13 connection to the requested value: -255 to +255
  11  *  that is received on subscribed topic motor1
  12  *
  13  * Note: connector labeled "INPUT" on sonar sensor goes to
  14  * digital 1 (bit 0), and connector labeled "OUTPUT" goes to
  15  * digital 2 (bit 1).
  16  */
  17 
  18 
  19 #include <ros.h>
  20 #include <std_msgs/Int32.h>
  21 #include <stdio.h>
  22 #include <unistd.h>
  23 #include <qegpioint.h>
  24 #include <qemotoruser.h>
  25 
  26 ros::NodeHandle  nh;
  27 std_msgs::Int32 range;
  28 ros::Publisher sonar1("sonar1", &range);
  29 CQEMotorUser &motor = CQEMotorUser::GetRef();   // motor singleton
  30 CQEGpioInt &gpio = CQEGpioInt::GetRef();                // GPIO singleton
  31 
  32 char *rosSrvrIp = "192.168.11.9";
  33 
  34 #define USPI 150
  35 #define BIAS 300
  36 
  37 /*
  38  * Motor callback - called when new motor speed is published
  39  */
  40 void motorCb(const std_msgs::Int32& motor13_msg){
  41         int speed = motor13_msg.data;
  42         printf("Received subscribed motor speed %d\n", speed);
  43     motor.SetPWM(0, speed);
  44 }
  45 ros::Subscriber<std_msgs::Int32> motorSub("motor13", motorCb );
  46 
  47 /*
  48  * Calculate difference in usec between sonar start & end timevals
  49  */
  50 unsigned long diff(struct timeval *ptv0, struct timeval *ptv1)
  51 {
  52         long val;
  53 
  54         val = ptv1->tv_usec - ptv0->tv_usec;
  55         val += (ptv1->tv_sec - ptv0->tv_sec)*1000000;
  56 
  57         return val;
  58 }
  59 
  60 /*
  61  * Sonar callback. Called at interrupt level when sonar output transitions,
  62  * indicating end of range measurement
  63  */
  64 void callback(unsigned int io, struct timeval *ptv, void *userdata)
  65 {
  66         static struct timeval tv0;
  67         static int flag = 0;
  68         int sonarVal;
  69 
  70         if (io==0)
  71         {
  72                 flag = 1;
  73                 tv0 = *ptv;
  74         }
  75 
  76         if (io==1 && flag)
  77         {
  78                 sonarVal = diff(&tv0, ptv);
  79                 if (sonarVal>BIAS)
  80                         sonarVal = (sonarVal-BIAS)/USPI;
  81                 range.data = sonarVal;
  82                 //printf("%d\n", sonarVal);
  83         }
  84 }
  85 
  86 int main()
  87 {
  88         volatile unsigned int d;
  89 
  90         /* initialize ROS & subscribers & publishers */
  91         //nh.initNode();
  92         nh.initNode(rosSrvrIp);
  93         nh.advertise(sonar1);   // advertise sonar range topic
  94         nh.subscribe(motorSub);         // subscribe to motor speed topic
  95 
  96         // reset bit 0, set as output for sonar trigger
  97         gpio.SetData(0x0000);
  98         gpio.SetDataDirection(0x0001);
  99 
 100         // set callbacks on negative edge for both bits 0 (trigger)
 101         // and 1 (echo)
 102         gpio.RegisterCallback(0, NULL, callback);
 103         gpio.RegisterCallback(1, NULL, callback);
 104         gpio.SetInterruptMode(0, QEG_INTERRUPT_NEGEDGE);
 105         gpio.SetInterruptMode(1, QEG_INTERRUPT_NEGEDGE);
 106 
 107         // trigger sonar by toggling bit 0
 108         while(1)
 109         {
 110                 gpio.SetData(0x0001);
 111                 for (d=0; d<120000; d++);
 112                 gpio.SetData(0x0000);
 113                 usleep(100000);         // the interrupt breaks us out of this sleep
 114                 usleep(100000);         // now really sleep
 115                 sonar1.publish( &range );
 116                 nh.spinOnce();
 117         }
 118 }

The code explained

The VEXProRangeMotorLoop program is only a small extension of the VEXProRangePublish program, having the addition of a motor speed subscriber. This section describes just the motor speed logic extensions; refer to the VEXProRangePublish description for the rest of the code. In fact, the motor speed subscriber uses the same logic previously described in the Example Subscriber tutorial, so we will only talk about how to control a motor using libqwerk.

  24 #include <qemotoruser.h>
  25 

In addition to the std_msgs/Int32.h message used to publish the requested motor speed, you include the libqwerk motor control library declarations, which provide access to the speed control methods.

  29 CQEMotorUser &motor = CQEMotorUser::GetRef();   // motor singleton
  30 

You get a reference to the motor control object, whose methods you will use.

  37 /*
  38  * Motor callback - called when new motor speed is published
  39  */
  40 void motorCb(const std_msgs::Int32& motor13_msg){
  41         int speed = motor13_msg.data;
  42         printf("Received subscribed motor speed %d\n", speed);
  43     motor.SetPWM(0, speed);
  44 }
  45 ros::Subscriber<std_msgs::Int32> motorSub("motor13", motorCb );

The motor speed callback is called whenever a message on the subscribed topic "motor13" is received. It takes the requested speed out of the Int32 message data and calls the motor's SetPWM() method. SetPWM() controls the pulse-width modulation of motor 13's H-bridge. The first argument is the motor index: 0 is motor 13, the first 2-wire motor port on the VEXPro. The valid range of speed values is -255 to +255, corresponding to full backward to full forward; 0 is stopped. See the libqwerk motor control library for a full list of available methods and arguments.

After defining the callback, the code instantiates the subscriber, telling it the topic name and callback function.

One other difference from VEXProRangePublish: this program sleeps in main() for only 100ms, giving it a much faster publish rate, and making the control loop faster.

That's it! Pretty straightforward. Next we'll look into the code that runs under ROS, and that subscribes to the range messages and publishes the motor speed messages to VEXProRangeMotorLoop.

VEXProRangeMotorCtrlr

This program runs as a ROS node on your ROS workstation and subscribes to rangefinder readings. Depending on whether the range is closer or further away than 20 inches, it runs the motor one way or the other - faster the further from the setpoint of 20 inches.

   1 #include "ros/ros.h"
   2 #include "std_msgs/Int32.h"
   3 
   4 int range = 20;
   5 
   6 void sonarCallback(const std_msgs::Int32::ConstPtr& sonarMsg)
   7 {
   8   ROS_INFO("sonar range: [%d]", sonarMsg->data);
   9   range = sonarMsg->data;
  10 }
  11 
  12 int
  13 main (int argc, char **argv)
  14 {
  15   // inialize ros
  16   ros::init (argc, argv, "VEXProRangeMotorCtrlr");
  17   ros::NodeHandle n;
  18   ros::Rate loop_rate (2);
  19   std_msgs::Int32 speedMsg;
  20   std_msgs::Int32 positionMsg;
  21 
  22   // declare the publishers & subscribers
  23   ros::Publisher motor13_pub =
  24     n.advertise < std_msgs::Int32 > ("motor13", 1000);
  25   ros::Subscriber sonarSub = n.subscribe("sonar1", 1000, sonarCallback);
  26 
  27   int speed = 0;
  28 
  29   while (ros::ok ())
  30     {
  31       speed = std::min(range, 40);      // clamp range to a max
  32       speed = (speed * 10) - 200;               // give it a range of -200 - +200, with 0 at 20"
  33       speedMsg.data = speed;
  34 
  35       ROS_INFO ("range %d speed %d", range, speed);
  36       motor13_pub.publish (speedMsg);
  37 
  38       ros::spinOnce ();
  39 
  40       loop_rate.sleep ();
  41     }
  42   return 0;
  43 }

The code explained

The VEXProRangeMotorCtrlr program runs on your ROS workstation. The executable is built when you rosmake the rosserial_embeddedlinux package on your ROS workstation, and is written into the bin directory.

   1 #include "ros/ros.h"
   2 #include "std_msgs/Int32.h"
   3 

As usual, you include the message types in use; in this case Int32 is used for both range and motor speed.

   4 int range = 20;

range is the current object distance; it is written by the subscriber callback function and read by the main() loop and motor control is based on this value.

   6 void sonarCallback(const std_msgs::Int32::ConstPtr& sonarMsg)
   7 {
   8   ROS_INFO("sonar range: [%d]", sonarMsg->data);
   9   range = sonarMsg->data;
  10 }

Just as on the VEXPro side, the ROS side uses callbacks to notify a subscriber that a message was received. sonarCallback() copies the range reading that was published by the VEXProRangeMotorLoop program on VEXPro into the range variable.

You can find an in-depth explanation of how this ROS subscriber code works in this tutorial.

  15   // inialize ros
  16   ros::init (argc, argv, "VEXProRangeMotorCtrlr");
  17   ros::NodeHandle n;
  18   ros::Rate loop_rate (2);
  19   std_msgs::Int32 speedMsg;
  20   std_msgs::Int32 positionMsg;

The node initializes its ROS subsystem. It gets a ROS Nodehandle, which gives access to methods to subscribe, publish, etc. It sets its loop-rate to 2Hz, and it declares the messages which will be used to receive range and send motor speed requests.

  22   // declare the publishers & subscribers
  23   ros::Publisher motor13_pub =
  24     n.advertise < std_msgs::Int32 > ("motor13", 1000);
  25   ros::Subscriber sonarSub = n.subscribe("sonar1", 1000, sonarCallback);

Next, the program instantiates the motor publisher and range subscriber, passing them the topic name and callback for the subscriber. Instantiating the publisher and subscriber causes them to ask the ROS master for the location of the node publishing or subscribing on those topics - this is the rosserial_python.py node instance acting as a proxy for VEXProRangeMotorLoop.

Note that the system supports multiple rosserial_python proxy nodes servicing (usually) different topics.

  29   while (ros::ok ())
  30     {
  31       speed = std::min(range, 40);      // clamp range to a max
  32       speed = (speed * 10) - 200;               // give it a range of -200 - +200, with 0 at 20"
  33       speedMsg.data = speed;
  34 
  35       ROS_INFO ("range %d speed %d", range, speed);
  36       motor13_pub.publish (speedMsg);
  37 
  38       ros::spinOnce ();
  39 
  40       loop_rate.sleep ();
  41     }

The program enters an endless loop, in which range is converted to speed in lines 31-32 by limiting it to 0 to 40 inches, then the distance range expanded to a speed range of -200 to +200. 20 inches produces a speed of 0.

The program publishes the motor speed, calls spinOnce() to let messages get sent, then sleeps for half a second in loop_rate.sleep() as long as ROS is running and it hasn't been terminated.

Testing the code

This section has three testing sections that use the programs described in this tutorial.

Testing the rangefinder

Plug the input connector of the VEX ultrasonic rangefinder into digital port 1, and the output connector into digital port 2. Like most ultrasonic rangefinders, the Vex unit uses a trigger pulse to start the ranging pulse, and emits a pulse when it detects the returning acoustic reflection. The time between the falling edge of the trigger and the falling edge of the output is the round-trip time of the acoustic wave, from which the range to the reflective surface can be calculated.

As usual, make sure ros_core and serial_node.py are running. Build, download, and start the VEXProRangePublish binary to the VEXPro controller.

Use rostopic to echo the range measurement (in inches) that's published by the VEXProRangePublish program once a second. Change the distance to the reflective surface and watch the range change.

$ rostopic echo sonar1
data: 38
---
data: 38
---
data: 39
---
data: 15
---
data: 15
---
data: 14
---

Testing the motor

Plug a 2-wire motor into the Motor13 port on the VEXPro. Note: Vex motors have plugs with male pins. You can adapt a 3rd-party motor by using a servo extender cable and some solder - just remove the sheath around the pins on the servo extender cable. The 2-wire motor connects to the two pins furthest from the edge of the controller (corresponding to the control and +5V pins of a servo or 3-wire motor). The controller pulse-width-modulates battery voltage to the motor pins through an H-bridge protected by a self-resetting fuse.

Build, download, and start the VEXProMotor13Subscribe binary to the VEXPro controller. Use rostopic to publish the desired motor speed to the listening subscriber. Note the use of -- preceding a negative speed. The valid speed range is -255 to +255.

$ rostopic pub -1 motor13 std_msgs/Int32 100
publishing and latching message for 3.0 seconds
$ rostopic pub -1 motor13 std_msgs/Int32 -- -100
publishing and latching message for 3.0 seconds
$ rostopic pub -1 motor13 std_msgs/Int32 0
publishing and latching message for 3.0 seconds

The program running on the VEXPro prints each speed as it receives it:

root@qwerk:~# /opt/usr/bin/VEXProMotor13Subscribe
Connecting to TCP server at 192.168.11.9:11411....
connected to server
Received subscribed motor speed 100
Received subscribed motor speed -100
Received subscribed motor speed 0

Pulling it all together: responsive control

This test builds on the two previous tests by running

  1. VEXProRangeMotorLoop on the VEXPro, which publishes sonar readings and subscribes to the motor speed topic
  2. VEXProRangeMotorCtrlr on the ROS workstation, which reads the sonar topic and responds by publishing motor speed/direction requests.

Build, download, and start the VEXProRangeMotorLoop binary to the VEXPro controller. It will sit publishing range and waiting for the first motor speed message.

root@qwerk:~# /opt/usr/bin/VEXProMotorRangeLoop
Connecting to TCP server at 192.168.11.9:11411....
connected to server

The VEXProMotorRangeCtrlr program was built when you build the embedded linux package, and the binary is in the rosserial_embeddedlinux/bin directory. Run it on your ROS workstation. The motor connected to the VEXPro will start running. Move a reflective surface nearer or further from the rangefinder and watch the motor speed and direction change.

$ rosrun rosseal_embeddedlinux VEXProRangeMotorCtrlr 
[ INFO] [1348455399.015726663]: range 20 speed 0
[ INFO] [1348455399.501250299]: range 20 speed 0
[ INFO] [1348455399.501761423]: sonar range: [28]
[ INFO] [1348455399.502253554]: sonar range: [28]
[ INFO] [1348455400.001241799]: range 28 speed 80
[ INFO] [1348455400.001640643]: sonar range: [28]
[ INFO] [1348455400.001790349]: sonar range: [28]
[ INFO] [1348455400.002211817]: sonar range: [28]
[ INFO] [1348455400.002438890]: sonar range: [28]
[ INFO] [1348455402.001361698]: range 27 speed 70
[ INFO] [1348455402.002070288]: sonar range: [26]
[ INFO] [1348455402.002388693]: sonar range: [25]
[ INFO] [1348455402.002700115]: sonar range: [23]
[ INFO] [1348455402.002869652]: sonar range: [22]
[ INFO] [1348455402.501255444]: range 22 speed 20
[ INFO] [1348455402.501669090]: sonar range: [21]
[ INFO] [1348455402.501814328]: sonar range: [21]
[ INFO] [1348455402.502216803]: sonar range: [20]
[ INFO] [1348455402.502413712]: sonar range: [19]
[ INFO] [1348455403.001236051]: range 19 speed -10
[ INFO] [1348455403.001656959]: sonar range: [19]
[ INFO] [1348455403.001812251]: sonar range: [18]
[ INFO] [1348455403.003050679]: sonar range: [18]
[ INFO] [1348455403.003198150]: sonar range: [17]
[ INFO] [1348455403.501292350]: range 17 speed -30
[ INFO] [1348455403.501671922]: sonar range: [17]
[ INFO] [1348455403.502039764]: sonar range: [17]
[ INFO] [1348455403.502313201]: sonar range: [16]
[ INFO] [1348455403.502652833]: sonar range: [16]
[ INFO] [1348455404.001357309]: range 16 speed -40
[ INFO] [1348455404.001723474]: sonar range: [16]
[ INFO] [1348455404.002102488]: sonar range: [15]
[ INFO] [1348455404.002332354]: sonar range: [15]
[ INFO] [1348455404.002524794]: sonar range: [15]
[ INFO] [1348455404.501225078]: range 15 speed -50

VEXProRangeMotorLoop running on the VEXPro prints the requested motor speed:

Received subscribed motor speed 0
Received subscribed motor speed 80
Received subscribed motor speed 70
Received subscribed motor speed 20
Received subscribed motor speed -10
Received subscribed motor speed -30
Received subscribed motor speed -40
Received subscribed motor speed -50

Wiki: rosserial_embeddedlinux/Tutorials/VEXPro range motor control loop (last edited 2012-09-25 10:37:46 by PaulBouchier)