Dual ultrasonic sensors with RobotC ?

sbdrobotics · April 7, 2013, 1:39pm

We have decided to try and use 2 ultrasonic range finders to square up with the trough in autonomous. The plan is to control the motors on each side with a seperate sensor in the event that there is any wheel slippage while making turns with our encoders.

I have looked but cannot find how to seperate the 2 without having each side caught in a loop of it’s own. Is it possible to have the 2 sensors working at the same time? I can do one at a time with no problem but that wont accomplish the desired task.

We are using RobotC

Thanks in advance!!!

Ruiqi_Mao · April 7, 2013, 2:05pm

you can create a separate task for each one or have both sensors run in the same loop.

sbdrobotics · April 7, 2013, 2:18pm

Thanks for the response! Given the fact that we have a whole 5 months experience, and only about 3 months experience with RobotC I’m still at a little bit of a loss. Some sample code (even for different sensors running in the same loop) would be super helpful! Prior to the teams programmer having an anxiety attack at our first competition and quiting the team I had not written a single line of code in over 20 years so I have a ton of cob webs to clear out!

I have been able to take the samples that JPearman has given me and master them enough to show them to my son who has written some nice encoder based routines but he has been too busy with the build to ask questions or try and figure it out himself.

edjubuh · April 7, 2013, 2:26pm

This is the code that I routinely use:

bool leftExit = false;
bool rightExit = false;

while((leftExit == false) || (rightExit == false)) { // while the left OR right side is not on target, do the following
	if(SensorValue[LeftEncoder] > goalValue) { motor[left] = -127; leftExit == false; } // we're not on target, so set the motor to go backwards and reset the leftExit variable back to false
	if(Sensorvalue[LeftEncoder] < goalValue) { motor[left] = 127; leftExit == false; } 
	else { motor[left] = 0; leftExit = true; } // we're on target, so the left side is OK to exit
	
	if(SensorValue[RightEncoder] > goalValue) { motor[right] = -127; rightExit == false; } // we're not on target, so set the motor to go backwards and reset the leftExit variable back to false
	if(Sensorvalue[RightEncoder] < goalValue) { motor[right] = 127; rightExit == false; } 
	else { motor[right] = 0; rightExit = true; } // we're on target, so the right side is OK to exit
}

Basically, you create one while loop that sees whether both sides are on target (by using the leftExit or rightExit boolean). Inside the while loop is your standard motor control. You can adapt it however you want, using PID if you’d like. I didn’t include it because it’s not necessary to show you how to achieve what you’re asking here.

Here is the code I actually use: (the one above should be less confusing)

while((leftExit == false) || (rightExit == false)) {
	if(SensorValue[EDL] > (leftTicks + EC_tolerance)) { 	leftOutput = -127; 	leftExit = false; }
	else if	(SensorValue[EDL] < (leftTicks - EC_tolerance)) { 	leftOutput =  127;	leftExit = false; }
	else {leftOutput =    0;  leftExit =  true; }

	if	(SensorValue[EDR] > (rightTicks + EC_tolerance)) { 	rightOutput = -127; rightExit = false; }
	else if (SensorValue[EDR] < (rightTicks - EC_tolerance)) { 	rightOutput =  127; rightExit = false; }
	else { rightOutput =    0; rightExit =  true; }
	chassis(leftOutput,rightOutput);
}

This code uses rudimentary tolerance control. I didn’t need to mess with refining a PID controller because what I have works. Besides, ain’t nobody got time fo dat. (jk, you should do PID if you have the time)

jpearman · April 7, 2013, 3:10pm

Quite possible, but there are many ways to skin a cat. You need to decide what you want the code to do before figuring out how to do it. Do you want to control motor speed or simply stop left and right at different times. Or you count rotate the robot until both sonars were reading the same distance.

Here is some code to get you going. First, not what you want to do but here is the function from our programming skills robot that we used to drive forwards a distance based on sonar readings.

void
DriveForwardsSonar( int distance, bool stopdrive = true )
{
    long    cur_position;
    long    target_position;
    long    currentTime;

    cur_position = SensorValue RearSonar ];

    target_position = cur_position + distance;

    // Start driving
    if(distance >= 0)
        DriveSystemDrive( 40, 0,  0 );
    else
        DriveSystemDrive( -40, 0, 0 );

    // wait for end - 5 seconds maximum
    for( currentTime = nSysTime; currentTime > (nSysTime - DEFAULT_DRIVE_TIMEOUT);)
        {
        cur_position = SensorValue RearSonar ];
        if( distance >= 0)
            {
            if( cur_position >= target_position )
                break;
            }
        else
            {
            if( cur_position <= target_position )
                break;
            }

        wait1Msec(10);
        }

    // Stop driving
    if( stopdrive )
        DriveSystemDrive( 0, 0, 0 );
}

A couple of features to note. It can drive forwards or backwards, driving is based on an offset from the current position. There is a timeout of DEFAULT_DRIVE_TIMEOUT (which was set to 2.5 seconds) so that we did not get blocked here if the sonar was not working.

So one way to achieve what you want may be something like this.

// placeholder, use slew rate control in reality
void
SetMotor( tMotor port, int speed )
{
    motor port ] = speed;
}

void
DriveToTroughSonar( )
{
    long    cur_left_position;
    long    cur_right_position;
    long    currentTime;

    cur_left_position = SensorValue LeftSonar ];

    // already there
    if( cur_left_position < 15 )
        return;

    // Start driving - assume forwards
    SetMotor( LeftMotor,  100 );
    SetMotor( RightMotor, 100 );
    
    // wait for end - 5 seconds maximum
    for( currentTime = nSysTime; currentTime > (nSysTime - 5000);)
        {
        cur_left_position  = SensorValue LeftSonar ];
        cur_right_position = SensorValue RightSonar ];
        
        // at 15cm stop
        if( cur_left_position < 15 )
            SetMotor( LeftMotor,  0 );   
            
        if( cur_right_position < 15 )
            SetMotor( RightMotor,  0 );   
            
        if( (cur_left_position < 15 ) && (cur_right_position < 15 ))
            break;

        wait1Msec(10);
        }

    // in case of timeout
    SetMotor( LeftMotor,  0 );   
    SetMotor( RightMotor,  0 );  
}

Start driving, check both sonars, stop left side and right side independently.

Another way.

// placeholder, use slew rate control in reality
void
SetMotor( tMotor port, int speed )
{
    motor port ] = speed;
}

void
DriveToTroughSonar( )
{
    long    cur_left_position;
    long    cur_right_position;
    long    currentTime;
    long    sonar_diff;
    
    cur_left_position = SensorValue LeftSonar ];

    // already there
    if( cur_left_position < 15 )
        return;

    // Start driving - assume forwards
    SetMotor( LeftMotor,  100 );
    SetMotor( RightMotor, 100 );
    
    // wait for end - 5 seconds maximum
    for( currentTime = nSysTime; currentTime > (nSysTime - 5000);)
        {
        cur_left_position  = SensorValue LeftSonar ];
        
        // stop at 15cm stop
        if( cur_left_position < 15 )
            break;
        
        wait1Msec(10);
        }

    // Stop motors
    SetMotor( LeftMotor,  0 );   
    SetMotor( RightMotor,  0 );
    
    // Now rotate 
    for( currentTime = nSysTime; currentTime > (nSysTime - 5000);)
        {
        cur_left_position   = SensorValue LeftSonar ];
        cur_right_position  = SensorValue RightSonar ];
        
        sonar_diff = cur_left_position - cur_right_position;
        
        // are we close enough?
        if( abs(sonar_diff) < 2 )
            break;
        
        if( sonar_diff > 0 )
            {
            // left larger - rotate right
            SetMotor( LeftMotor,  30  );   
            SetMotor( RightMotor, -30 ); 
            }
        else
            {
            // right larger - rotate left
            SetMotor( LeftMotor,  -30  );   
            SetMotor( RightMotor, 30 ); 
            }
            
        wait1Msec(10);
        }
   
    // Stop motors
    SetMotor( LeftMotor,  0 );   
    SetMotor( RightMotor,  0 );    
}

This drives forward and then rotates to align with the trough.

I did not test either of these functions so use with caution. Sonar was set to cm not inches.

drdanielfc · April 8, 2013, 9:26am

Just a heads-up that RobotC gets confused with multiple ultrasonic range finders.

EDIT:
Sorry accidentally Posted the wrong link before - here’s a thread with more details: http://www.robotc.net/forums/viewtopic.php?t=2665

jpearman · April 8, 2013, 9:43am

Have you experienced this? I’ve used two before and never had any problems, although they were not pointing in the same direction. The Ultrasonic sensors are polled approximately 20 times per second but that varies depending on the distance. After the ping is initiated, the receiver will wait up to about 36mS for a reply, there is a delay of 50mS before the next ping. Each ultrasonic is polled in turn so with two sensors each will be updated about 10 times per second.

drdanielfc · April 8, 2013, 9:49am

I have experienced this although they were facing in the same direction which was likely the problem in hindsight. I haven’t used the sonar sensors since last year so I don’t have any recent experience with them.

sbdrobotics · April 8, 2013, 6:03pm

Here is what I have been trying, it seems as though both sensors have to be on target for it to stop, it does not control the wheels independently. I can wave my hand over either sensor and nothing happens, but if I wave over both then it stops. I am trying to get the 2 sides seperated so it will square up at the trough.

#pragma config(Sensor, in1,    arm,            sensorPotentiometer)
#pragma config(Sensor, dgtl1,  sonarRight,     sensorSONAR_inch)
#pragma config(Sensor, dgtl5,  encoderRight,   sensorQuadEncoder)
#pragma config(Sensor, dgtl8,  sonarLeft,      sensorSONAR_inch)
#pragma config(Sensor, dgtl10, encoderLeft,    sensorQuadEncoder)
#pragma config(Motor,  port2,           frontRight,    tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port3,           backRight,     tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor,  port4,           liftRight,     tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port5,           intakeRight,   tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor,  port6,           frontLeft,     tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port7,           backLeft,      tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port8,           liftLeft,      tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port9,           intakeLeft,    tmotorServoContinuousRotation, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task main()

{
	    //Clear Encoders
  SensorValue[encoderLeft] = 0;
  SensorValue[encoderRight] = 0;
  
  while(SensorValue[encoderLeft] < 300)
  {
    //...Move forward.
    motor[frontLeft] = 63;
    motor[backLeft] = 63;
    motor[frontRight] = 63;
    motor[backRight] = 63;
  }
   wait1Msec(250);
   //Clear Encoders
  SensorValue[encoderLeft] = 0;
  SensorValue[encoderRight] = 0;
  
  while(SensorValue[encoderLeft] < 250)
  {
    //...90 degree right turn.
    motor[frontLeft] = 63;
    motor[backLeft] = 63;
    motor[frontRight] = -63;
    motor[backRight] = -63;
  }
  
  wait1Msec(250);
  
  //Clear Encoders
  SensorValue[encoderLeft] = 0;
  SensorValue[encoderRight] = 0;
  
  while(SensorValue[encoderLeft] < 200)
  {
    //...Move forward.
    motor[frontLeft] = 63;
    motor[backLeft] = 63;
    motor[frontRight] = 63;
    motor[backRight] = 63;
  }
  
   wait1Msec(250);
  
  //Clear Encoders
  SensorValue[encoderLeft] = 0;
  SensorValue[encoderRight] = 0;
  
  while(SensorValue[encoderRight] < 250)
  {
    //...90 dgree left turn.
    motor[frontLeft] = -63;
    motor[backLeft] = -63;
    motor[frontRight] = 63;
    motor[backRight] = 63;
  }
  
   wait1Msec(250);
   
   {
     motor[intakeRight] = 127;
   }
  
  //Clear Encoders
  SensorValue[encoderLeft] = 0;
  SensorValue[encoderRight] = 0;
  
  while(SensorValue[encoderLeft] < 750)
  {
    //...Move forward.
    motor[frontLeft] = 40;
    motor[backLeft] = 40;
    motor[frontRight] = 40;
    motor[backRight] = 40;
  }
  
  wait1Msec(1000);
  
   //Clear Encoders
  SensorValue[encoderLeft] = 0;
  SensorValue[encoderRight] = 0;
  
  while(SensorValue[encoderRight] >= -25)
  {
    //...Back away from sack stack.
    motor[frontLeft] = -63;
    motor[backLeft] = -63;
    motor[frontRight] = -63;
    motor[backRight] = -63;
  }
  
  
  wait1Msec(750);
  
{
	motor[intakeRight] = 0;
}
  wait1Msec(250);
   //Clear Encoders
  SensorValue[encoderLeft] = 0;
  SensorValue[encoderRight] = 0;
  
  while(SensorValue[encoderLeft] < 275)
  {
    //...90 degree right turn.
    motor[frontLeft] = 63;
    motor[backLeft] = 63;
    motor[frontRight] = -63;
    motor[backRight] = -63;
  }
 
    //Clear Encoders
  SensorValue[encoderLeft] = 0;
  SensorValue[encoderRight] = 0;
  
  while(SensorValue[encoderRight] >= -575)
  {
    //...Move towards left side of field.
    motor[frontLeft] = -63;
    motor[backLeft] = -63;
    motor[frontRight] = -63;
    motor[backRight] = -63;
  }
   wait1Msec(250);
   //Clear Encoders
  SensorValue[encoderLeft] = 0;
  SensorValue[encoderRight] = 0;
  
  while(SensorValue[encoderLeft] < 255)
  {
    //...90 degree right turn.
    motor[frontLeft] = 63;
    motor[backLeft] = 63;
    motor[frontRight] = -63;
    motor[backRight] = -63;
  }
  
	while(SensorValue[sonarLeft] >2)
  	{
  		motor[frontLeft] = -63;
  		motor[backLeft] = -63;
  	
  	}
  	

while(SensorValue[sonarRight] >2)
  		{
  			motor[frontRight] = -63;
  			motor[backRight] = -63;
  		
  		}
  
  //Loop Forever
  while(1 == 1)
  {
    //Remote Control Commands
    motor[frontRight] = vexRT[Ch2] - vexRT[Ch1];
    motor[backRight] =  vexRT[Ch2] + vexRT[Ch1];
    motor[frontLeft] = vexRT[Ch3] - vexRT[Ch4];
    motor[backLeft] =  vexRT[Ch3] + vexRT[Ch4];

    //Arm Control
    if(vexRT[Btn5U] == 1)
    {
      motor[liftLeft] = 127;
      motor[liftRight] = 127;
    }
    else if(vexRT[Btn5D] == 1)
    {
      motor[liftLeft] = -63;
      motor[liftRight] = -63;
    }
    else
    {
      motor[liftLeft] = 0;
      motor[liftRight] = 0;
    }

    //Intake Control
    if(vexRT[Btn6U] == 1)
    {
      motor[intakeLeft] = 127;
      motor[intakeRight] = 127;
    }
    else if(vexRT[Btn6D] == 1)
    {
      motor[intakeLeft] = -127;
      motor[intakeRight] = -127;
    }
    else
    {
      motor[intakeLeft] = 0;
      motor[intakeRight] = 0;
    }
  }
}

jpearman · April 8, 2013, 6:14pm

Lets look at the last few lines of code before you enter driver control.

     //Clear Encoders
    SensorValue[encoderLeft] = 0;
    SensorValue[encoderRight] = 0;
  
    while(SensorValue[encoderLeft] < 255)
        {
        //...90 degree right turn.
        motor[frontLeft]  = 63;
        motor[backLeft]   = 63;
        motor[frontRight] = -63;
        motor[backRight]  = -63;
        }
  
    while(SensorValue[sonarLeft] >2)
        {
        motor[frontLeft] = -63;
        motor[backLeft]  = -63; 	
        }
  	
    while(SensorValue[sonarRight] >2)
        {
        motor[frontRight] = -63;
        motor[backRight]  = -63;
        }

You are turning right, left motors at 63, right motors at -63. The first while loop ends, next the first sonar loop starts, you put the left motors at -63 BUT the right motors are still running at -63 as well so the robot backs up. The test says do that while the sonar is grater than 2, which is probably is, so the loop never ends unless you wave your hand. See if you can fix it, otherwise I will look at it again later.

sbdrobotics · April 8, 2013, 6:16pm

Is there a way to get the rigt and left sonars and motors running at the same time but seperately?

drdanielfc · April 8, 2013, 6:22pm

The code should be more like this (note: untested, just wrote this in gedit)

bool leftMove = true;
bool rightMove = true;

while (leftMove || rightMove)
{
	if (SensorValue[sonarLeft] <= 2)
		leftMove = false;
	if (SensorValue[sonarRight] <= 2)
		rightMove = false;
	
	if (leftMove)
		motor[frontLeft] = -63;
	else
		motor[frontLeft] = 0;

	if (rightMove)
		motor[frontRight] = -63;
	else
		motor[frontRight] = 0;
}

jpearman · April 8, 2013, 6:22pm

Yes, I guess the code I posted yesterday didn’t help. I need to eat dinner now but let me try again with your code, I will post something for you later.

drdanielfc · April 8, 2013, 6:25pm

Does my response not answer the question?

sbdrobotics · April 8, 2013, 6:31pm

Thanks to both of you! I will try what was posted, but to be honest yesterdays code lost me a bit. To be honest we probably should have started with easyC with no more experience then we have in programming, but we are gradually working through it and have gotten a good handle on what has been taught to us so far.

Once we get past worlds we will be taking some time to try and learn the ins and outs of RobotC so we can work through more issues on our own.

drdanielfc · April 8, 2013, 6:38pm

This is my own personal opinion but honestly I really don’t see how "Easy"C is any easier than RobotC. The thing that people tend to have the toughest time with is thinking through step-by-step what the code is going to do, not typing vs dragging commands. I really don’t think that the right mentality is any easier to learn in one environment or another - it just takes some patience to learn things.

jpearman · April 8, 2013, 7:13pm

Some questions.

How far from the trough do you want the sonars to be? They don’t work that well close up, you had two inches in the code, are they really going to be that close? Are you starting further away than the final desired distance ?

sbdrobotics · April 8, 2013, 7:26pm

We stuck 2" in as a random number to change later. We will be around 36" from the trough to begin with but will need to get pretty close since the robot is a “back dumper”. When we get to a field where we can test the finished code we will adjust the number accordingly. I am sure there will be some over-run so we will need to make that number bigger. We are toying with the idea of using encoders to get us about 5 or 6 inches away and then using the sonar at a lower travel speed to close in on the trough.

sbdrobotics · April 8, 2013, 7:44pm

I tried Daniels code and it compiled after I added some braces. It still shuts down both motors with either sensor being close enough, I am sure I have missed something that would be simple to one with more knowledge than myself, but I can’t seem to get a handle on it. Below is what I have.

bool leftMove = true;
bool rightMove = true;

while (leftMove || rightMove)
{
	if (SensorValue[sonarLeft] <= 2)
		leftMove = false;
	if (SensorValue[sonarRight] <= 2)
		rightMove = false;
	
	if (leftMove)
		{
motor[frontLeft] = -63;
motor[backLeft] = -63;
}
	else
{
	
motor[frontLeft] = 0;
motor[backLeft] = 0;
}

	if (rightMove)
	{
motor[frontRight] = -63;
motor[backRight] = -63;}
	else
	{	
motor[frontRight] = 0;
motor[backRight] = 0;}
}

sbdrobotics · April 8, 2013, 7:48pm

Got it working, the code was correct. Thanks!