Dual ultrasonic sensors with RobotC ?

[jpearman]

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.

Ok, try this. You can ignore all the other changes I made (and did not test), I know you didn’t ask me to change anything else but I wanted to show you some of the types of things you could do to make the code more modular. Find the sonar stuff near the end, again, I cannot actually try this today so it may be wrong but it should give you the idea. I will send some more comments privately.

#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               !!*//

// This is the sonar distance from the trough we want to attain
#define  TROUGH_DISTANCE    6

// A timeout in case the encoders fail - set larger than you expect
// driving to take, currently 7 seconds
#define  DRIVE_TIMEOUT    7000

// Function to set drive motor speed
void
SetDriveMotors( int leftSpeed, int rightSpeed )
{
    motor[frontLeft]  = leftSpeed;
    motor[backLeft]   = leftSpeed;
    motor[frontRight] = rightSpeed;
    motor[backRight]  = rightSpeed;
}

// A function to drive using the left encoder to measure distance
//
void
DriveUsingLeftEncoder( int distance, int leftSpeed, int rightSpeed )
{
    //Clear Encoders
    SensorValue[encoderLeft] = 0;
    SensorValue[encoderRight] = 0;
    
    // Use T1 as timeout
    time1[T1] = 0;
    
    if( distance > 0 )
        {
        while((SensorValue[encoderLeft] < distance) && (time1[T1] < DRIVE_TIMEOUT))
            SetDriveMotors( leftSpeed, rightSpeed );
        }
    else
        {
        while((SensorValue[encoderLeft] >= distance) && (time1[T1] < DRIVE_TIMEOUT))
            SetDriveMotors( leftSpeed, rightSpeed );
        }
}

// A function to drive using the right encoder to measure distance
//
void
DriveUsingRightEncoder( int distance, int leftSpeed, int rightSpeed )
{
    //Clear Encoders
    SensorValue[encoderLeft]  = 0;
    SensorValue[encoderRight] = 0;

    // Use T1 as timeout
    time1[T1] = 0;
    
    if( distance > 0 )
        {
        while((SensorValue[encoderRight] < distance) && (time1[T1] < DRIVE_TIMEOUT))
            SetDriveMotors( leftSpeed, rightSpeed );
        }
    else
        {
        while((SensorValue[encoderRight] >= distance) && (time1[T1] < DRIVE_TIMEOUT))
            SetDriveMotors( leftSpeed, rightSpeed );
        }
}


task main()
{
    //...Move forward.
    DriveUsingLeftEncoder( 300, 63, 63 );
    wait1Msec(250);
    //...90 degree right turn.
    DriveUsingLeftEncoder( 300, 63, -63 );
    wait1Msec(250);
    //...Move forward.
    DriveUsingLeftEncoder( 200, 63, 63 );
    wait1Msec(250);
    //...90 dgree left turn.
    DriveUsingRightEncoder( 250, -63, 63 );
    wait1Msec(250);

    // Start Intake - seems like we needed left as well
    motor[intakeLeft]  = 127;
    motor[intakeRight] = 127;

    //...Move forward.
    DriveUsingLeftEncoder( 750, 40, 40 );
    wait1Msec(1000);

    //...Back away from sack stack.
    DriveUsingRightEncoder( -25, -63, -63 );
    wait1Msec(750);

    // Stop intake - seems like we needed left as well
    motor[intakeLeft]  = 0;
    motor[intakeRight] = 0;
    wait1Msec(250);

    //...90 degree right turn.
    DriveUsingLeftEncoder( 275, 63, -63 );

    //...Move towards left side of field.
    DriveUsingRightEncoder( -575, -63, -63 );
    wait1Msec(250);

    //...90 degree right turn.
    DriveUsingLeftEncoder( 255, 63, -63 );

    // Stop
    SetDriveMotors( 0, 0 );

    // Take a breath
    wait1Msec(1000);

    // Ok - so here is the sonar code
    // Drive backards until both sonars are TROUGH_DISTANCE inches from the trough
    // Stop left or right independently

    // Start driving backwards - don't know what speed
    SetDriveMotors( -50, -50 );

    // do this until both are within distance, may need to slow down
    while( (SensorValue[sonarLeft] > TROUGH_DISTANCE) || (SensorValue[sonarRight] > TROUGH_DISTANCE) )
        {
        // If the left one triggers
        if( SensorValue[sonarLeft]  <= TROUGH_DISTANCE )
            {
            // Stop left
            motor[frontLeft]  = 0;
            motor[backLeft]   = 0;
            }

        // If the right one triggers
        if( SensorValue[sonarRight] <= TROUGH_DISTANCE )
            {
            // Stop right
            motor[frontRight]  = 0;
            motor[backRight]   = 0;
            }
            
        // Don't hog cpu
        wait1Msec(10);
        }

    // Make sure all motors are stopped
    SetDriveMotors( 0, 0 );


    // Off into driver control
    //Loop Forever
    while(1 == 1)
        {
        //Remote Control Commands
        
        // Ok so I added a deadband for you as well       
        if( (abs(vexRT[Ch1]) > 10) || (abs(vexRT[Ch2]) > 10) || (abs(vexRT[Ch3]) > 10) || (abs(vexRT[Ch4]) > 10))
            {
            motor[frontRight] = vexRT[Ch2] - vexRT[Ch1];
            motor[backRight]  = vexRT[Ch2] + vexRT[Ch1];
            motor[frontLeft]  = vexRT[Ch3] - vexRT[Ch4];
            motor[backLeft]   = vexRT[Ch3] + vexRT[Ch4];
            }
        else
            {
            motor[frontRight] = 0;
            motor[backRight]  = 0;
            motor[frontLeft]  = 0;
            motor[backLeft]   = 0;
            }
            
        //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;
            }
        }
}

[sbdrobotics]

Ok, try this. You can ignore all the other changes I made (and did not test), I know you didn’t ask me to change anything else but I wanted to show you some of the types of things you could do to make the code more modular. Find the sonar stuff near the end, again, I cannot actually try this today so it may be wrong but it should give you the idea. I will send some more comments privately.

Thanks! We will try this tomorrow.

[drdanielfc]

Got it working, the code was correct. Thanks!

The original was correct, right?

Also, jpearman’s code is most certainly better so I would start working off of that (if you have the time.) Also a heads up that I forgot to include the “special case” where the sensors return -1 whenever the sensor doesn’t receive the sound back. Usually one just ignores when this happens so a fix for mine would be:

bool leftMove = true;
bool rightMove = true;

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

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

[jpearman]

The original was correct, right?

Also, jpearman’s code is most certainly better so I would start working off of that (if you have the time.) Also a heads up that I forgot to include the “special case” where the sensors return -1 whenever the sensor doesn’t receive the sound back. Usually one just ignores when this happens so a fix for mine would be:

I don’t know about better, it’s different but we actually wrote more or less the same thing. I sort of ignored the -1 case, although I think the while loop will end if -1 is read as that will be less than the desired distance anyway. It’s always good to see slightly different ways of doing the same thing, over time we all develop our own style and it becomes subjective as to which style is better. I’m fairly old school and use lots of white space, I make it even more verbose with code I post here as I think it helps with understanding.

I tend to think that even with the two ultrasonics you may end up at an angle to the trough, stopping the motors on one side will tend to rotate the robot before you can get the other side stopped quickly enough, slow speed control of these robots is hard (a speed controller issue with the way MC29s work, not software).

btw, I updated my previous post as there was a bug.

Edit:
for the benefit of others reading, if the ultrasonic sensor returns -1 it means that there is no valid reading, usually the distance is too large or too small.

Edit2:
Which also make me think, how are the ultrasonic sensors angled? The trough is a pretty small target so -1 may be seen often, I would probably have used them on the front and aimed at the field perimeter.

[drdanielfc]

I don’t know about better, it’s different but we actually wrote more or less the same thing. I sort of ignored the -1 case, although I think the while loop will end if -1 is read as that will be less than the desired distance anyway. It’s always good to see slightly different ways of doing the same thing, over time we all develop our own style and it becomes subjective as to which style is better. I’m fairly old school and use lots of white space, I make it even more verbose with code I post here as I think it helps with understanding.

I tend to think that even with the two ultrasonics you may end up at an angle to the trough, stopping the motors on one side will tend to rotate the robot before you can get the other side stopped quickly enough, slow speed control of these robots is hard (a speed controller issue with the way MC29s work, not software).

btw, I updated my previous port as there was a bug.

Edit:
for the benefit of others reading, if the ultrasonic sensor returns -1 it means that there is no valid reading, usually the distance is too large or too small.

Well better in the sense that you used better convention and a more robust design for the whole program whereas I was just demonstrating the logic for that one part.

As for yours, as long as both range finders don’t return -1 on the same iteration then it will still work.

In terms of programming style, I like to use whitespace just to make parts clearer, but not necessarily putting it everywhere. For example, I like to use it to make order of operations clearer. Ie:

x = x + 2./3 - 5

I also really appreciate your verbosity. It really helps when trying to figure out what’s actually going on in your code or just because it makes it easier to jump to a certain section (ie the “Rotate 90 degrees” section)

What I would do to get more accurate positioning would likely be making it slow before stopping (this is more for the OP, not jpearman, who knows most of this). You could do this proportionally based on the distance (ie Multiply the speed by the distance remaining) but it could be tricky to come up with the right equation. If you’re having trouble then you can do the simpler hack of making it a speed of 60 for greater than 5 inches away, 50 for 5 inches, 40 for 4, 30 for 3, and then stop at 2 (these are just examples).

If you need help implementing something like this please ask

EDIT (in response to jpearman’s edit):

Edit2:
Which also make me think, how are the ultrasonic sensors angled? The trough is a pretty small target so -1 may be seen often, I would probably have used them on the front and aimed at the field perimeter.

The only problem with that is that things could get really tricky if the opposite team’s robot were to wander in the way of yours. I would mount them on the back and aim at your own perimeter so you can be sure that there won’t be interference.

Back to my earlier post, I can really see these two sensors interfering with each other’s signals so if you’re having trouble with 2 but one works correctly, you (we?) may need to come up with some way to delay the reading from one to the other.

[jpearman]

The only problem with that is that things could get really tricky if the opposite team’s robot were to wander in the way of yours. I would mount them on the back and aim at your own perimeter so you can be sure that there won’t be interference.

That’s what I meant, sbdrobotics is driving backwards so the front of the robot is aimed at the perimeter on his side of the field.

Back to my earlier post, I can really see these two sensors interfering with each other’s signals so if you’re having trouble with 2 but one works correctly, you (we?) may need to come up with some way to delay the reading from one to the other.

The ultrasonics are used sequentially, the first one sends the ping and then waits up to 36mS for the echo, after a further 50mS delay, the second one is pinged and the reply waited for. This sequence then repeats, not sure if they ever implemented using more than two.

Some scope traces showing ultrasonic timing here.
https://vexforum.com/showpost.php?p=322622&postcount=2

[jpearman]

I’m having trouble seeing how this actually works in practice. Lets look at the geometry.

[ATTACH]7383[/ATTACH]

[ATTACH]7381[/ATTACH]

The green square represents a 15" cube (the robot), there are two ultrasonic sensors on the front face, one points directly forwards, the other upwards at an angle.

The forward facing sensor (1) will not detect the face of the trough which is angled down, any waves hitting it will be reflected towards the floor rather than back towards the robot. They may be able to detect the stanchion but it’s very narrow and will not reflect much. The angled sensor (2) will not detect anything when the robot is in this position.

[ATTACH]7382[/ATTACH]

Now the robot is much closer, the forward sensor still is not able to detect the trough, the angled sensor now will see the trough wall about 6 inches away. This sensors output will change very quickly from detecting nothing (-1) to finding the trough.

[sbdrobotics]

Maybe this will make more sense.

http://sbdrobotics.com/wp-content/uploads/2013/04/untitled.png

[jpearman]

Maybe this will make more sense.

Understood, I wasn’t suggesting that you had two sensors installed in the way I was showing them, I was just interested to see what would happen with the sensor detecting parallel to the floor or angled upwards.

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”.

So the code will need to ignore the ultrasonic readings until the robot is almost under the trough, probably going to need a combination of encoder driving and then use the ultrasonics when valid data is obtained.

[sbdrobotics]

Understood, I wasn’t suggesting that you had two sensors installed in the way I was showing them, I was just interested to see what would happen with the sensor detecting parallel to the floor or angled upwards.

So the code will need to ignore the ultrasonic readings until the robot is almost under the trough, probably going to need a combination of encoder driving and then use the ultrasonics when valid data is obtained.

Thats what we decided last night after the sonar code was working. We will use encoder based movement until we are around 6" or so from the trough and then move slowly backwards until we are in position. We tried a couple of different routines paying attention to the time in motion and finally found the shortest distance to travel and still be lined up for an accurate shot at the sacks. It looks like we get near the trough fast enough to spend a few seconds making sure we are in position.

We started with the left wheels on a power expander and the right ones on the cortex which worked fine as soon as the batteries came off the charger, but lost accuracy in turns after about 10 minutes. We are going to seperate front and back instead of left and right which is something that should have dawned on us to begin with. Otherwise we will put all drive motors on one source.

[jpearman]

We started with the left wheels on a power expander and the right ones on the cortex which worked fine as soon as the batteries came off the charger, but lost accuracy in turns after about 10 minutes. We are going to seperate front and back instead of left and right which is something that should have dawned on us to begin with. Otherwise we will put all drive motors on one source.

There is no magic bullet for power distribution, most of the time I put all of the drive on the cortex, left on ports 2&3, right on ports 8&9, this spreads the drive over two PTC devices. You could then use ports 4, 5, 6 & 7 on the power expander for the lift and intake. If the lift uses a potentiometer for feedback be careful how the cortex is powered off when using a backup battery, the pot can lose power if the cortex is turned off, however, code is still running due to the backup battery and will read incorrect pot values, this may then feed into lift control which still has power via the power expander. I personally like ports 1&10 for arm/lift motors, PID works better on these ports than the others.

[Team80_Giraffes]

One of our teams 80P had a sonar in the point to the trough this year too.

They used encoders to continue to drive and do an underside slam dunk once they were past the trough. Sounds like you are a front dump mechanism so you back up instead of go forward.

It worked well, but what worked better for them was they had a gyro straighten them out before they went forward and did the scoring motion.

Depending upon the configuration of your scorer, you may miss more often than you’d like unless you shore yourself up to be on the 90 degree angles of the field and then attempt to score.

• Drive until trough detected (their scoop would have gotten the sacks under the trough as their sensor was at the arm joint, not way up front like your cube drawing)
• straighten yourself out to get to 0 degrees
• go forward X clicks
• lift the arm and slam dunk the sacks in the trough
• lower arm

You can also put this on a button for use in driver control! (But be sure to give yourself a way out to get back to full user control.)

[drdanielfc]

Thats what we decided last night after the sonar code was working. We will use encoder based movement until we are around 6" or so from the trough and then move slowly backwards until we are in position. We tried a couple of different routines paying attention to the time in motion and finally found the shortest distance to travel and still be lined up for an accurate shot at the sacks. It looks like we get near the trough fast enough to spend a few seconds making sure we are in position.

We started with the left wheels on a power expander and the right ones on the cortex which worked fine as soon as the batteries came off the charger, but lost accuracy in turns after about 10 minutes. We are going to seperate front and back instead of left and right which is something that should have dawned on us to begin with. Otherwise we will put all drive motors on one source.

This is likely because when you’re turning, you’re taking into consideration how far the robot is going to roll after you kill power. This is where slowing down for the last couple inches is going to make a big difference because then you won’t get that rolling when you have charged batteries and there shouldn’t be much of a distance on weaker batteries.

[edjubuh]

You could always use two line sensors to align yourself at the line preceding the trough. Since the trough and tape lines are parallel, so long as the robot goes forward, you won’t have any issues. Any potential turns would be slight and likely insignificant. I don’t know your robot, so maybe you have those issues.

After lining up on that line, just have the robot go forward and have ultrasonic stop it, as you have it in your code.

You now don’t have to worry about gyro compound errors and ultrasonics interfering with each other.

This was the way I planned on completing such a task.