SONAR Sensor Software Driver

Thanks for posting the driver code. I had to finnagle the timer interrupt routine a tad to get it to work on the full-size FRC, but otherwise it has made my job much easier!

Thanks! :slight_smile:

Yeah, thanks alot Mike. It works perfectly.

This saved me hours of coding, and I’m sure it will do the same for many others.

Why is this code so complicated? I really don’t see why you need all of this. I don’t know anything about programming the range finder, but it seems to me that you should just be able to send the range finder a 0 or a 1 to trigger it and then measure with a timer the time it takes for the pulse to come back. Why is there all this complicated stuff? Is it really needed? I’m not criticizing, I just want to know for my own sake.

I did not write the code but I have read it and it does not seem to have any unnecessary parts. In my opinion it is written very cleanly and only contains the minimum amount of code needed to correctly use an ultrasonic range finder. You are right on target about what must be done to use a range finder, however I believe once you start coding it you will have a very similar solution. The only thing that I see that can be taken out of this code is some of the error checking but error checking is good practice since it means that you have thought of and prepared for things that can go wrong.

Since I wrote it I can answer most questions you have on it. For the most part as Alan said the driver does not have any unnecessary lines of code. However, I can address some of the issues that make it a little more complicated than most pieces of software.

  • Two I/O pins being used- this was a necessary evil that I found while writing the software for the Parallax PING))) sensor. I just couldn’t get a single I/O pin to change between an output to an input with an interrupt fast enough. I would lose the sonar sensors signal. I didn’t know about the Vex Robotics range finder until after originally writing the software. The VEX and Parallax units are very similar if not identical. I think the Vex engineers may have ran into the same problem I did and decided to use two cables.
  • Design- There’s a variable that holds the current state of the sonar and functions in the source code file to handle a hardware interrupt and a timer interrupt. The other option would have been one function which triggers the sonar sensor and then loops until the sonar sensor is finished responding. The problem with that design would be that you would be wasting processor time for around 20 mS waiting. This means no other code could be running, the input/output might not happen and the Master uP could think your user program has gone haywire and shut it down.

Any other questions, feel free to ask :wink:

I have an updated version of the SONAR software driver that supports the 2006 Robot Controller. Get it here:

Thank you, thank you, thank you! for writing this code. I’m planning to try it soon (I found a VEX sonar unit at RadioShack last night).

I’ve been scanning the code to understand it a little. And I found something strange: at one point sonar_status is set to SONAR_REST, but there is no case statement looking for SONAR_REST. In fact, SONAR_REST appears nowhere else in the code except in the .h file where it is created.

Is SONAR_REST a remnant from a previous version? Won’t the code get lost if sonar_status is set to SONAR_REST?


The code doesn’t get lost when the status is set to SONAR_REST. Shortly after the status is set to rest on line 206 the timer is started. In the timer interrupt service routine the status is set to SONAR_OFF. After that occurs, the sonar can be pulsed again with Ping_Sonar(). I found the rest was necessary to allow the sensor to work correctly.

I’m glad you like the software. Please let us know how your testing turns out.

A couple of quick questions. I’m installing the code without the benefit of hardware to try it on (that will be in a couple of days).

In your README it says

  1. Add the following code to the if block in user_routines_fast.c/InterruptHandlerLow()

else if (PIR2bits.TMR3IF && PIE2bits.TMR3IE)
I did, and I think I did it correctly (code is below).

But step 3 puzzles me.

3.) Add Hardware_Interrupt_Sonar() to the if statement that handles the INT2 pin
in user_routines_fast.c/InterruptHandlerLow()

My attempt to interpret step three also appears in the code below.

Here is my modified InterruptHandlerLow (). The mods are in color.

#pragma code
#pragma interruptlow InterruptHandlerLow save=PROD /* You may want to save additional symbols. */

void InterruptHandlerLow ()     
  unsigned char int_byte;       
  if (INTCON3bits.INT2IF && INTCON3bits.INT2IE)       /* The INT2 pin is RB2/DIG I/O 1. */
    Hardware_Interrupt_Sonar();  ////// <--- added to support sonar
    INTCON3bits.INT2IF = 0;
  else if (INTCON3bits.INT3IF && INTCON3bits.INT3IE)  /* The INT3 pin is RB3/DIG I/O 2. */
    INTCON3bits.INT3IF = 0;
  else if (INTCONbits.RBIF && INTCONbits.RBIE)  /* DIG I/O 3-6 (RB4, RB5, RB6, or RB7) changed. */
    int_byte = PORTB;          /* You must read or write to PORTB */
    INTCONbits.RBIF = 0;     /*     and clear the interrupt flag         */
  }                                        /*     to clear the interrupt condition.  */
  // the following added to support sonar:
  else if ( PIR2bits.TMR3IF && PIE2bits.TMR3IE )
    CheckUartInts();    /* For Dynamic Debug Tool or buffered printf features. */

Comments appreciated… including comments on i/o pin conflicts that I’m not noticing… Thanks!

Instead of a new thread I’ll post here. There is a new sonar ranging sensor on the market. New sonar The interesting points as I see it are - Nice price, narrower cone of detection, and 3 ways to interface. RS232, analog voltage and the normal pulse timing. Too late for this year but may be a worthy addition to next years sensor selection.

Perfect :slight_smile:

How do I connect the PING))) sensor made for Parallex robots to the VEX Micro Controller and program the sensor in EasyC 2.0?

IT WORKS! Cool. Thanks so much for the code.

NOTE: ERROR IN VEX SONAR DATA SHEET! The data sheet that comes with the VEX sonar kit has an error. It says,

The connector labelled “OUTPUT” is the trigger output of the Vex microcontroller;…
The connector Labelled “INPUT” is the echo response from the ultrasonic detector; this is the line through which the Vex microcontroller receives output from the detector, …

Well, that seemed backwards to me, but I carefully hooked up the “OUTPUT” to the digital_io_16 port on the FRC, which your code uses to signal the unit to ping, and hooked the “INPUT” to the INT2 (digital_io_1) port on the FRC. It didn’t work. I checked everything else, then switched the two wires. Then it worked. THE VEX DOCUMENTATION IS BACKWARDS. The device’s wire marked “output” is indeed the output of the device.

One way to tell that it’s hooked up right and the software is pinging it is to hold the device up to your ear and listen to it. When I got it working I could hear a faint “tictictictic…” coming from the device. I did not hear that before I switched the wires.

Thanks again. Now I have to write new autonomous code that I can’t test on the robot until we get to the next regional.

Just a further update on the sonar system. We were successful in using the sonar to control our distance to the corner goal (our doors won’t open if we’re too close, and the balls won’t go in if we’re too far away). We used sonar both in autonomous and as a driver control (driver initiates “autonomous” using a button. Control is returned when the balls have been dumped or the driver hits the “abort” button.) We even got a Xerox Creativity Award for the system. :smiley:

But here’s why I’m writing this note…

We had a strange problem. The sonar system would give steady values until we enabled autonomous, and then the values would start to drop: 150, 150, 150, [start auto] 150, 150, 130, 115, 90, 75, … All this while the robot was on blocks and unable to move!

I figured it was some bizzare resource interrupt limitation timing thing. Until I discovered that it would do this just by me running the motors. It took a mechanical engineer to put his hand on the frame and say, “I bet it’s picking up vibration from the motors!” We removed the sonar unit and held it – the problem went away. We ended up mounting the unit on a block of foam (a block we made by cutting up one of those balls from the game).

Oddly, the autonomous code seemed to work anyway. It only did this for long distances – at short distances ( < 30" ) we didn’t see this problem.

So, everyone — be careful how you mount the sonar unit – it can pick up vibrations and give bad readings!

Thanks again for the driver code – we never would have been able to complete this in time without it!


What were you trying to read a distance off when this happened? Sometimes the sensor gets screwy readings when it is pointed at a curved object. Silly sound waves.

We were pointing it at a wall in the Drexel gym. Big, flat and perpendicular to the robot’s axis. The test we did of removing the sonar unit from the robot and just holding it there, an inch from where it was mounted, seemed to be conclusive proof that it was indeed vibration pickup from the robot. Further, mounting it on foam stopped the problem.

SPEAKING OF MOUNTING IT ON FOAM: be careful how you mount it to the foam. Just putting a block of foam behind the unit and screwing it down through the foam doesn’t work: the screws transmit the vibration from the frame directly to the sonar unit. You must mount the unit to the foam, then mount the *foam * to the robot’s frame. This gives isolation.

Darn it, I went and started a new thread before I saw this post!
In my defense: Gdeaver’s post didn’t show up on a search.

Anyway: The code to measure the pulse is very cool, but that new sensor also gives an analog voltage at 10 mV/inch - just a simple analog input and you’re done…


This may just be on one of those days again but how do you hook up the vex sonar sensor to the EDU robot.

You need to adapt the male connections on the Vex ultrasonic sensor to the male connections on the EDU. Either a female-female cable or replace the Vex ultrasonic cable ends with female connectors. You can also use those Y-cables that come in the FRC KOP each year to do the job.

Other than that you hook it up to the same digital output and interrupt input as used on the Vex.

I was compiling SONAR_0.3 using MPLAB C18 and it was not successful.

Followings are the errors detected from the codes in sonar.c.

At function initialize_sonar()

    –>symbol ‘TRISJbits’ has not been defined
    –>struct or union object designator expected
    –>lvalue required

    –>symbol ‘LATJbits’ has not been defined
    –>struct or union object designator expected
    –>lvalue required

At function ping_sonar ()

    –>symbol ‘LATJbits’ has not been defined
    –>struct or union object designator expected
    –>lvalue required

    –>symbol ‘LATJbits’ has not been defined
    –>struct or union object designator expected
    –>lvalue required

I thought maybe the problem lies with the definition in sonar.h for both SONAR_OUTPUT_CONFIG and SONAR_OUTPUT (see following 2 lines)

define SONAR_OUTPUT rc_dig_out16

define SONAR_OUTPUT_CONFIG digital_io_16

It seems to me that the define statements is referring to the hardware part or maybe the MPLAB C18 compiler is unable to read it?

Would anyone reading this can share which compiler they use and how to go about solving this problem?

pls help. Thank you. :slight_smile: