Hard lesson learned & request for code refinement

we are trying to use the ultrasonic sensor for auton.

Spent hours beating head against the wall trying to figure what the auton would run and then not. Long story short we had to insert a 200ms delay upon start to allow time for the sonar to initialize

This being said we are having a hard time getting consistent stopping distance and hoping for some insight. We’ve tried variants of other’s code w/little improvement.

This is what we have so far:
(we also have a variable dist_mm which reads the sensor every 50ms - is it better to use this than make system calls to hardware?)

void drive2Target(double target) { // drive by spin
bool b = 1;
double speed = 20;



      L_Drive.spin(vex::directionType::fwd, speed, vex::velocityUnits::pct);
      R_Drive.spin(vex::directionType::rev, speed,vex::velocityUnits::pct); 
    if(Dist.distance(vex::distanceUnits::mm) > target * 4){
      speed = 50;
   } else if(Dist.distance(vex::distanceUnits::mm) < target * 4 && Dist.distance(vex::distanceUnits::mm) > target + 50){
     speed = 20;     
    } else {
      b = 0;
} // end drive2Target

I can see the 200nms being needed, as it takes time to get the reading. Here is some info:

We tried to use it last year at one practice. Mind you… we tried only for a short time, but there seems to be some time lag for the sensor due to the physics of what its doing - sending out a sound pulse, then receiving it back and interpreting the distance. We were trying to read the sensor dynamically while moving at full speed and by the time the cortex got the information it was too old to react to and did the wrong thing. If you were moving slower, maybe it would work better.

What we are planning on doing next is driving to a supposed correct position, stopping, then after a short delay, read the sensor and hopefully it will be stable by that time to get a short correction move calculated. Then, make the correction and continue with auton.


Your autonomous is not based on reaching a position but instead running a motor until an event has occured. This creates very low precision, as the robot’s velocity can vary and the rangefinder can experience noise and errant values. It is what makes flappy bird so hard. It would be much better to control the position of the robot than it is to control the “throttle” per se.

For this reason, you will want to use the build in position meters in the motors. Tell them to go to a position and stop.

The ultrasonic range finder is great to see how far away the side walls are, how far away the next cube is, or if you are the proper distance away from a tower to score. They are great for measuring unknown distances, not known distances. The motor encoders know how far they have gone, and do so with much more resolution.


Thanks - good explanation

yea, tried to use the encoders (spinFor) based off of the sonar (degRotate = dist_mm / circumference) but that failed miserably

will re-visit this though!

great links - thanks

ah, interesting… I believe the V5 is much faster than the cortex but surely all the bouncing could affect … going to order the inertia sensor and use the drivetrain object…

so I would recommend writing a small PD loop, with a timeout parameter.
float kd=...; float ki=...; int threshold=...;

void drivepid(int targetvalue, int timeout) {
[reset timer and lasterr]

while (true) {
if (timer>timeout || speed+err<threshold) { drivetrain.stop; break;}

or just use built in drive train commands to use the built in PIDs in the motors:

void drivefor(int distance, int speed, int timeout, str stopping, bool wait) {
drivetrain.setTimeout(timeout); drivetrain.setStopping(stopping); drivetrain.setVelocity(speed); drivetrain.driveFor(forward, distance, inches, wait);
and then just use this function to simplify your auton to a few instances of drivefor( ):
drivefor(18, 50, 3.5, coast, false);

just a few ideas.


Thanks!!! I’ll look into this!

I managed to get it consistently +15mm using the encoder #/revs based on sensor. Maybe good enough for now but hoping the inertia sensor & you ideas will help!!!

No problem. Timeouts are most teams’ worst enemy when it comes to autonomous, so you could try

  • recording how far you went forwards as d and then use -d in your next drive command, or
  • constantly check for the total time to be >14.00 seconds since auton started, in which your robot switches to “plan b” and just tries to score anything instead of 0)

If you set good drive speeds with respect to your intake (no plowing blocks), use maximum drive speeds otherwise, use good timeout values (and develop alternate sets of actions in case your value is way off after a timeout), put small pauses in between steps when you need stuff to settle down (so nothing flings out), and get a good tilter velocity curve (stepping down the velocity as it tilts upwards), you can create a very reliable autonomous without anything too advanced, like s curves and odometry.

Best of luck.

Best of luck.


wow - some great info!!!

re: d and -d : not sure I understand entirely. We’re driving to distance, say 200mm (spinFor xxx degrees) to a cube. It’s do or die - we’re either on target or not.

Right now we’re reading the sonar, and calculating the number of degrees to rotate based on that. V5 is spot-on in driving these steps.

Interesting side-note: On every iteration I always ended up having to ‘hard-code’ 50mm into these calcs. Turns out our drive train has ~50mm of slop! Hopefully the inertial sensor can compensate for this or a quad encoder on a wheel?

The one thing I’ve struggled with is varying the velocity of the motors. I found a lot of V4 examples but been unable to acheive same w/V5. Is seems once the motor.spin () command starts it ignores the changes in the speed variable. I was able to do this by using a speed equation if/else inside the spin() but seemed very dumb & ugly.

Agree, pauses can not be overstated!!!

Thanks and best of luck to you & your team!

motor.spin() gets it going, so you may need to set the velocity and then motor.spin() again.

50mm of slop sounds pretty typical for shaft coming out of a motor mated to a 4" wheel. The shaft coming out of the motor is loosely held which introduces several degrees of slop, and on top of that, there is always some play with anything mounted on the standard shafts. An encoder wheel would definitely fix it, but at the cost of realestate within the chasis, and horrifying results when going over obstacles. Since that is not the case this year, they are a good call if you can find the room for them.

Otherwise, the best way to fix a simple direct drivetrain is just use HS shafts, (RIP mechanum wheels).
Alternately, bolt a 3.25" omniwheel to a HS 60T gear, and then drive that gear with a 36T gear driven by a 600 RPM motor. One of our teams tried that and got a bot that had an equivalent speed to a 4" wheel running at 290 rpm, but with very, very little slop. However that made for a horrifically fast robot. Much better for turning point than TT. Otherwise, the same effect can be had with a 1:1 gearing mounted above the wheel+gear.

1 Like

as always - you posts are great & well received.

Sadly our wheel set-up is a 2-motor, 4WD in the following config:
4"omni w/12T <-> 60t <-> motor w/12T <-> 60t <-> 60t <-> 4"omni w/12T
…so I guess 2" slop isn’t bad considering…

Good point about the shaft/collar slop - will try blue-loctite-ing them this weekend. Should help somewhat. Be a pain to service it but so far our drive has been the least maintenance item…

Maybe to the wire-tie-motor-thing in case of burn out

1 Like