Ultrasonic Sensor Hard Code Help

Technic-R-C · July 6, 2007, 7:35pm

I was reading an issue of robot magazine (page 48, spring 2007 issue) and a code came up that was used in the Parallax Boe bot that drove through a maze. Would anyone know how to make this code work using C.

Here is the code summary;

Ultrasonic sensor measures the front distance,
Ultrasonic sensor measures the distance to the right,
Ultrasonic sensor measures the distance to the left,
(only one ultrasonic sensor is used and it rotates using a motor)
(How do I store the distances for later use in the code?)
Is distance forward > 7 inches ?
If yes, then move forward 6 inches and go to start.
Is distance right> distance left and > 7 inches?
If yes, then turn right and go to Start.
Is distance left > distance rigght and > 7 inches?
If yes, then turn left and go to start
Otherwise turn 180 degrees and go to start.

This code is fairly complicated. I really need help on the first part(1-3). I am not able to store the distances in the front left and right. Should I stop the ultrasonic sensor after each directional scan and then Start the sensor again after the ultrasonic sensor turns? Should I use any Wait blocks? The rest of the code is a number of If, Else If, and Else statements.

All help is greatly appreciated!

Good Luck!

theotherguy · July 6, 2007, 8:24pm

question - are you writing this in MPLab or EasyC?

What is complicated about storing distances? (sorry, I don’t know your skill level in C)

Technic-R-C · July 7, 2007, 8:24am

I am using Easy C.
I try to store the variables by starting and stopping the ultrasonic sensor in each direction. This makes the code really long, is there any shortcut?

theotherguy · July 7, 2007, 8:42am

I’ve done this before (a long time ago, and I don’t think it worked for me). It really depends what version of EasyC you have. v1 is no good, and you’ll just have to live with the long code, but if you have v2 or higher (or so I’m told, I don’t have v2 yet), you could make a function and save yourself a lot of code.

pacoliketaco · July 7, 2007, 10:55am

this is a test program that hopefully will help you:

Technic-R-C · July 7, 2007, 6:49pm

Thanks pacoliketaco,
I programmed the robot with a similar program and it works. I have one more question. How do you add RX control of 2 more motors while the current code is working? I have tried putting in RX control but I am not able to control the robot with the controller while the code is being executed. Is the Vex microcontroller capable of executing more than one code at a time?

pacoliketaco · July 10, 2007, 5:02pm

you’re welcome.

i would personally make some sort of switch, say maybe a rc switcher that can toggle back and forth between the two modes. here is a basic rc switcher, the same kind that i use in almost every single one of my robots:

just ignore the switcher variable, that works with the other functions in this program

Technic-R-C · July 12, 2007, 8:44pm

Thanks,

Regarding the code that I stated at the beggining of the post, I decide to allow the robot to move forward until the ultrasonic sensor reads less than 10 The only problem is that the code keeps on starting over before the forward moving part finishes. Can you please help, the code is below

#include “UserAPI.h”

int loop = 1;
int ultrasonic;
int f; //forward distance
int l; //left distance
int r; //right distance
int limit;

void main ( void )
{
//Connect Input Wire to Digital Output 11
//Connect Output wire to Interrupt 1
//Near = 2, Far = 100
StartUltrasonic ( 1 , 11 ) ; // interrupt 1= output label on sensor, DO11=input label on sensor
while ( loop == 1 )
{
//measures front distance
f = GetUltrasonic ( 1 , 11 ) ;
PrintToScreen ( “distance recorded\n” ) ;
Wait ( 2000 ) ;
//turns right
PrintToScreen ( “turns right\n” ) ;
SetMotor ( 4 , 75 ) ;
Wait ( 2300 ) ;
SetMotor ( 4 , 127 ) ;
r = GetUltrasonic ( 1 , 11 ) ;
PrintToScreen ( “distance recorded\n” ) ;
Wait ( 2000 ) ;
//turns to the left
PrintToScreen ( “turns left\n” ) ;
SetMotor ( 4 , 180 ) ;
Wait ( 4700 ) ;
SetMotor ( 4 , 127 ) ;
l = GetUltrasonic ( 1 , 11 ) ;
PrintToScreen ( “distance recorded\n” ) ;
Wait ( 2000 ) ;
SetMotor ( 4 , 75 ) ;
Wait ( 2285 ) ;
SetMotor ( 4 , 127 ) ;
PrintToScreen ( “sensor is pointing in the forward direction\n” ) ;
Wait ( 1000 ) ;
if ( f > 25 )
{
ultrasonic = GetUltrasonic ( 1 , 11 ) ;
if ( ultrasonic > 10 )
{
SetMotor ( 1 , 255 ) ;
SetMotor ( 2 , 48 ) ;
}
else
{
SetMotor ( 1 , 127 ) ;
SetMotor ( 2 , 127 ) ;
}
//moves forward about 60 cm
}
else if( r > l && r > 20)
{
PrintToScreen ( “Robot is turning to the right\n” ) ;
SetMotor ( 1 , 245 ) ;
SetMotor ( 2 , 245 ) ;
Wait ( 225 ) ;
SetMotor ( 1 , 127 ) ;
SetMotor ( 2 , 127 ) ;
}
else if( l > r && l > 20)
{
PrintToScreen ( “robot is turning to the left\n” ) ;
SetMotor ( 1 , 10 ) ;
SetMotor ( 2 , 10 ) ;
Wait ( 225 ) ;
SetMotor ( 1 , 127 ) ;
SetMotor ( 2 , 127 ) ;
}
else
{
PrintToScreen ( “robot turns 180 degrees\n” ) ;
SetMotor ( 2 , 200 ) ;
SetMotor ( 1 , 200 ) ;
Wait ( 750 ) ;
SetMotor ( 1 , 127 ) ;
SetMotor ( 2 , 127 ) ;
}
}
}

Technic-R-C · July 13, 2007, 8:09am

The part of the code that does not execute fully before the program starts over is below. In other words the code below is running while the ultrasonic sensor begins scanning again. Can someone please help.

if ( f > 25 )
{
ultrasonic = GetUltrasonic ( 1 , 11 ) ;
if ( ultrasonic > 10 )
{
SetMotor ( 1 , 255 ) ;
SetMotor ( 2 , 48 ) ;
}
else
{
SetMotor ( 1 , 127 ) ;
SetMotor ( 2 , 127 ) ;
}
//moves forward about 60 cm
}

MarkO · July 13, 2007, 1:39pm

Technic-R-C:

The part of the code that does not execute fully before the program starts over is below. In other words the code below is running while the ultrasonic sensor begins scanning again. Can someone please help.
if ( f > 25 )
{
    ultrasonic = GetUltrasonic ( 1 , 11 ) ;
    if ( ultrasonic > 10 )
    {
        SetMotor ( 1 , 255 ) ;
        SetMotor ( 2 , 48 ) ;
    }
    else
    {
        SetMotor ( 1 , 127 ) ;
        SetMotor ( 2 , 127 ) ;
    }
    //moves forward about 60 cm
}

EasyC 1.x or 2.x?? If you attach you EasyC Project, that makes it easer to test and Trouble-Shoot… Especially if I only have 5 Minutes to look at the problem and give you an answer. If you “wrap” your Code in code tags (e.g. code ] / code ]) it will keep the tabbed indenting that was copied from EasyC.

Technic-R-C · July 13, 2007, 2:51pm

Here is the code again with the code tags. Do you know whats wrong.


#include "UserAPI.h"

int loop = 1; 
int ultrasonic; 
int f; 
int l; 
int r; 
int limit; 

void main ( void )
{
      //Connect Input Wire to Digital Output 11
      //Connect Output wire to Interrupt 1
      //Near = 2, Far = 100
      StartUltrasonic ( 1 , 11 ) ; // interrupt 1= output label on sensor, DO11=input label on sensor
      while ( loop == 1 )
      {
            //measures front distance
            f = GetUltrasonic ( 1 , 11 ) ;
            PrintToScreen ( "distance recorded\n" ) ;
            Wait ( 2000 ) ;
            //turns right
            PrintToScreen ( "turns right\n" ) ;
            SetMotor ( 4 , 75 ) ;
            Wait ( 2300 ) ;
            SetMotor ( 4 , 127 ) ;
            r = GetUltrasonic ( 1 , 11 ) ;
            PrintToScreen ( "distance recorded\n" ) ;
            Wait ( 2000 ) ;
            //turns to the left
            PrintToScreen ( "turns left\n" ) ;
            SetMotor ( 4 , 180 ) ;
            Wait ( 4700 ) ;
            SetMotor ( 4 , 127 ) ;
            l = GetUltrasonic ( 1 , 11 ) ;
            PrintToScreen ( "distance recorded\n" ) ;
            Wait ( 2000 ) ;
            SetMotor ( 4 , 75 ) ;
            Wait ( 2285 ) ;
            SetMotor ( 4 , 127 ) ;
            PrintToScreen ( "sensor is pointing in the forward direction\n" ) ;
            Wait ( 1000 ) ;
            if ( f > 25 )
            {
                  ultrasonic = GetUltrasonic ( 1 , 11 ) ;
                  if ( ultrasonic > 10 )
                  {
                        SetMotor ( 1 , 255 ) ;
                        SetMotor ( 2 , 48 ) ;
                  }
                  else
                  {
                        SetMotor ( 1 , 127 ) ;
                        SetMotor ( 2 , 127 ) ;
                  }
                  //moves forward about 60 cm
            }
            else if( r > l && r > 20)
            {
            PrintToScreen ( "Robot is turning to the right\n" ) ;
            SetMotor ( 1 , 245 ) ;
            SetMotor ( 2 , 245 ) ;
            Wait ( 225 ) ;
            SetMotor ( 1 , 127 ) ;
            SetMotor ( 2 , 127 ) ;
            }
            else if( l > r && l > 20)
            {
            PrintToScreen ( "robot is turning to the left\n" ) ;
            SetMotor ( 1 , 10 ) ;
            SetMotor ( 2 , 10 ) ;
            Wait ( 225 ) ;
            SetMotor ( 1 , 127 ) ;
            SetMotor ( 2 , 127 ) ;
            }
            else
            {
            PrintToScreen ( "robot turns 180 degrees\n" ) ;
            SetMotor ( 2 , 200 ) ;
            SetMotor ( 1 , 200 ) ;
            Wait ( 750 ) ;
            SetMotor ( 1 , 127 ) ;
            SetMotor ( 2 , 127 ) ;
            }
      }
}

I have Easy C 1.x

theotherguy · July 13, 2007, 10:42pm

Technic-R-C:

The part of the code that does not execute fully before the program starts over is below. In other words the code below is running while the ultrasonic sensor begins scanning again. Can someone please help.
if ( f > 25 )
{
ultrasonic = GetUltrasonic ( 1 , 11 ) ;
if ( ultrasonic > 10 )
{
SetMotor ( 1 , 255 ) ;
SetMotor ( 2 , 48 ) ;
}
else
{
SetMotor ( 1 , 127 ) ;
SetMotor ( 2 , 127 ) ;
}
//moves forward about 60 cm
}

I think what’s happening here is it is using the if statement, causing the robot to set the motor and then continue on with the program. You probably need to replace it with a while loop like so:


if ( f > 25 )
{
while(ultrasonic > 10)
{
ultrasonic = GetUltrasonic ( 1 , 11 ) ;
SetMotor ( 1 , 255 ) ;
SetMotor ( 2 , 48 ) ;
}
SetMotor ( 1 , 127 ) ;
SetMotor ( 2 , 127 ) ;
//moves forward about 60 cm
}

pacoliketaco · July 14, 2007, 10:50am

or, using the if statement, you need to have it wait something short like 100 ms, before stopping the motors in the same if statement. i know this makes a longer code, but it works.

theotherguy · July 14, 2007, 10:57am

But doing this does not help. The robot will drive forward for 1/10th of a second, not by the ultrasonic sensor.

pacoliketaco · July 14, 2007, 11:03am

my reasoning was that when the ultrasonic has nothing in its way, it will move slightly forward, and then it will do a complete scan again. i prefer using two ultrasonic sensors and then there is no time wasted scanning.

or like theotherguy said, you could use a while loop.

Technic-R-C · July 14, 2007, 11:31am

I tried using the code with the while statement but the robot completely ignored this part of the code, even if the forward direction is greater than 25. I tried putting the GetUltrasonic in fron of the while statement. This allowed the robot to keep driving forward without stopping, even if it is less than 10 ultrasonic units away from the wall. I understand the concept of allowing the robot to drive forward for 1/10th of a second (I already tried this), but I do not want to use this method in my robot do to the inaccuracy and wasted time scanning. Is there anyting else you can do to the code to make the code work?

Technic-R-C · July 14, 2007, 11:57am

Here is the robot that the code is being used with.

corpralchee · July 14, 2007, 4:58pm

I love that robot, it’s so small…

Technic-R-C · July 14, 2007, 5:34pm

Thanks, I designed it to be really really really small. There are only 2 metal parts used in the robot, not including the bars, gussets and the beams. I wanted to make something that was small and efficient.

I still need help with the code.

MarkO · July 16, 2007, 12:49pm

Thanks for the Code Tags, your code is much easer to follow…

What is the Behavior that you are looking for from your Vex??? Start with what you want it to do, then Code that…

As pointed out by theotherguy, if you want to keep moving while the Forward UltraSonic value is Greater Than 25, until it is NOT Greater than 10, you need to keep your program in one area. The While Loop will do that…

Lets break you “program flow” down to see what it does…

Initilazation Stuff

#include "UserAPI.h"

int loop = 1; 
int ultrasonic; 
int f; 
int l; 
int r; 
int limit; 

void main ( void )
{
      //Connect Input Wire to Digital Output 11
      //Connect Output wire to Interrupt 1
      //Near = 2, Far = 100
      StartUltrasonic ( 1 , 11 ) ; // interrupt 1= output label on sensor, DO11=input label on sensor

ALL of the following code is Loop Part, that executes over and over until the power is lost.

This part executes Every Time through the Loop

      while ( loop == 1 )
      {
            //measures front distance
            f = GetUltrasonic ( 1 , 11 ) ;
            PrintToScreen ( "distance recorded\n" ) ;
            Wait ( 2000 ) ;
            //turns right
            PrintToScreen ( "turns right\n" ) ;
            SetMotor ( 4 , 75 ) ;
            Wait ( 2300 ) ;
            SetMotor ( 4 , 127 ) ;
            r = GetUltrasonic ( 1 , 11 ) ;
            PrintToScreen ( "distance recorded\n" ) ;
            Wait ( 2000 ) ;
            //turns to the left
            PrintToScreen ( "turns left\n" ) ;
            SetMotor ( 4 , 180 ) ;
            Wait ( 4700 ) ;
            SetMotor ( 4 , 127 ) ;
            l = GetUltrasonic ( 1 , 11 ) ;
            PrintToScreen ( "distance recorded\n" ) ;
            Wait ( 2000 ) ;
            SetMotor ( 4 , 75 ) ;
            Wait ( 2285 ) ;
            SetMotor ( 4 , 127 ) ;
            PrintToScreen ( "sensor is pointing in the forward direction\n" ) ;
            Wait ( 1000 ) ;

This part is the Conditionals, because you chose the IF ELSE-IF ELSE-IF ELSE, ONE and only ONE of these will Execute each time through the Loop.

IF ‘f’ is Greater than 25, this block of code will execute…

             if ( f > 25 )
            {
                  ultrasonic = GetUltrasonic ( 1 , 11 ) ;
                  if ( ultrasonic > 10 )
                  {
                        SetMotor ( 1 , 255 ) ;
                        SetMotor ( 2 , 48 ) ;
                  }
                  else
                  {
                        SetMotor ( 1 , 127 ) ;
                        SetMotor ( 2 , 127 ) ;
                  }
                  //moves forward about 60 cm
            }

So IF ‘f’ is NOT Greater than 25,
Then test ‘r’ for Greater than ‘l’ and Greater than 20, if true this block of code will execute…

             else if( r > l && r > 20)
            {
            PrintToScreen ( "Robot is turning to the right\n" ) ;
            SetMotor ( 1 , 245 ) ;
            SetMotor ( 2 , 245 ) ;
            Wait ( 225 ) ;
            SetMotor ( 1 , 127 ) ;
            SetMotor ( 2 , 127 ) ;
            }

So IF ‘f’ is NOT Greater than 25, AND ‘r’ is NOT Greater than ‘l’ and NOT Greater than 20,
Then test ‘l’ is Greater than ‘r’ AND ‘l’ is Greater than 20, if true this block of code will execute…

             else if( l > r && l > 20)
            {
            PrintToScreen ( "robot is turning to the left\n" ) ;
            SetMotor ( 1 , 10 ) ;
            SetMotor ( 2 , 10 ) ;
            Wait ( 225 ) ;
            SetMotor ( 1 , 127 ) ;
            SetMotor ( 2 , 127 ) ;
            }

And so IF ‘f’ is NOT Greater than 25, AND ‘r’ is NOT Greater than ‘l’ and NOT Greater than 20, AND ‘l’ is NOT Greater than ‘r’ AND ‘l’ is NOT Greater than 20, this block of code will execute…

             else
            {
            PrintToScreen ( "robot turns 180 degrees\n" ) ;
            SetMotor ( 2 , 200 ) ;
            SetMotor ( 1 , 200 ) ;
            Wait ( 750 ) ;
            SetMotor ( 1 , 127 ) ;
            SetMotor ( 2 , 127 ) ;
            }
      }

The Ending Brace…