Let's Make Robots!

Need help with a steering algorithm...


This may be more of a general programming question than arduino specific; I guess the principles apply to all programming languages.

I'm having a bit of trouble with programming the reaction to the sensors on my GrasshopperBot...  Up until now I've had non-ranging IR sensors: as soon as an obstruction comes into the detection range of the sensor, the output of the sensor goes low.  This is nice and easy to program for, as it's just a digital on/off signal - if the input goes off, you turn the car away from the obstruction.

However, I now have a set of 4 ranging ultrasonic sensors (HC-SR04) and I want the arduino to react differently depending on how far away an obstruction is.  E.g., if an object is 50cm away from the left hand sensor, the car steers right by only a small amount until the obstacle is cleared; if the object is 5cm away however, it steers sharply away.  I want the arduino to be able to calculate the steering value dynamically without having to use some sort of pre-defined lookup table.  This way I don't have to re-calibrate any lookup tables if I increase the speed; at the moment it only runs at a very low speed, but eventually I want to be able to run the car at full speed.  And full speed on this thing is pretty damn fast...

The steering only has a range of +/- 35 degrees from the neutral (90 degree) position - the servo itself can go from 0 to 180 degrees, but the chassis design restricts the range of motion.  To steer left the value has to be below 90 degrees, and to steer right the value has to be above 90.

I settled on a detection threshold of 35cm while testing to keep the calculations in integers; floating point values will introduce extra computational complexity, which I can do without at this point.  If the principle works then I can easily change the detection threshold later on to a different value (although I'd need to convert floats to integers, as you probably can't write a float to a servo).  I'm also ignoring forward/reverse motion until I get the steering nailed.

The algorithm that I first came up with went along the lines of this - please forgive any syntax errors here, as I don't have the compiler here to tell me off for getting things wrong.  I've included definitions for any variables with a fixed value (I will convert these to constants), but left out all the void setup, etc, to make things easier


int detectionthreshold = 35;

int rangeofservomotion = 35;

int neutral = 90;

leftsensorreading = leftsensor.Ranging(CM); //takes a sensor reading and returns a value in CM

if (leftsensorreading < detectionthreshold)


 while (leftsensorreading < detectionthreshold)


  steeringservoposition = neutral + ((detectionthreshold - leftsensorreading) * (rangeofservomotion / detectionthreshold));

  leftsensorreading = leftsensor.Ranging(CM);


 steeringservoposition = neutral;



However when I tested this, it often got stuck in a loop and the steering would never be reset to neutral, even with no obstructions.  I've already tested the sensors independently of the bot and they work just fine.

Is there a better way of doing this, or have I messed something up royally?  I can usually write code no problem, but when it comes to mathematical algorithms I often stare at the screen blankly and start dribbling.

Comment viewing options

Select your preferred way to display the comments and click "Save settings" to activate your changes.

void Avoid() {
  int startAvoid = 50; //farthest distance to initiate a turn in cm
  int avoid = 5; //closest distance to initiate a turn
  int minimumPercentageTurn = 0; //used in autoscale
  int maximumPercentageTurn = 100; //used in autoscale
  int neutral = 90; //center position on the servo
  int maxTurn = 35; //the farthest from center the servo can move
  int percentTurn; //hold the return value of autoscale
  int distance; //sensor response in cm   

   distance = leftsensor.Ranging();   

   while(distance <= startAvoid) {
      percentTurn = autoscale( avoid, startAvoid, minimumPercentageTurn, maximumPercentageTurn, distance );      

      int servoAdjust = 0; //used to adjust the servo      

      servoAdjust = percentTurn * maxTurn;      

      servo.write(servoAdjust); //generic servo adjustment      

      distance = leftsensor.Ranging();

With what I wrote, you will also need the servo lib and the autoscale lib.

Thanks for that, I've had a read through the autoscale() library.  Don't really fully understand the at the moment, but I'll see if I can follow it through and make sense of it.  I did try to create a library for autoscale, complete with .h & .cpp files, but I *really* don't know C++ so I got a bit lost!  Although, according to this Arduino forum post, autoscale has now been built in to the core library of functions and named map(), so I may not need to write my own library; in either case it'll compile if I just include the autoscale() code in my sketch...

I'll have a go with this tomorrow with the actual hardware, rather than just "on paper".  The missus and I have been out for dinner this evening, and I'm rather too full of food to concentrate properly at the moment!


Many thanks.

map indeed does look almost like autoscale. They both take the same control variables, just in different orders. Just swap map for autoscale and move the variables around as required.

Instead of:

percentTurn = autoscale( avoid, startAvoid, minimumPercentageTurn, maximumPercentageTurn, distance );


percentTurn = map ( distance, avoid, startAvoid, maximumPercentageTurn, minimumPercentageTurn );

No, the variables are not mixed up. I got them wrong in the first code segment. In my original you would turn harder the farther you are from the object ( !good ) :).

Otherwise, what I wrote 'might' work.

Instead of map or autoscale you can do this: 

percentTurn = (distance-avoid)/(startAvoid-avoid)*100

Ok I'm not sure if this would work but you could try finding a mulitplier. You could make this simpler by just saying you want for example 50 cm to be your threshold and you want 10 steps which gives you 5cm step width. Now find a multiplier by dividing the maximum servo angle, 125° by the number of steps.

Hope this is somewhat understandable :)

const int max_servo_angle = 125; // this one isn't really used, just informational
const double multiplier = 12.5; // gives you 10 steps, culate with 125 / desired steps
const int step_width = 5; // step width in cm
const int threshold = 50; // minimum distance to object in cm

int servoAdjust;

distance = leftsensor.Ranging();

while(distance < threshold){
    /* to determine the angle:
       distance / step_width; gives the number of steps that the distance contains
       the number of steps is then multiplied by the precalculated multiplier, which determines
       the resoloution of the whole system
    servoAngle = multiplier * (distance / step_width);

A bit off topic but maybe of your interest is the relation of the Grasshopper's velocity to the avoidance maneuver. You write about recalibration that you don't want to make when you increase the car's velocity. You mention this regarding lookup-tables. 

But it is kind of obvious that the maneuver must be different when you approach an obstacle with 4km/h or 40km/h. So the treshold you use is relative to the current speed, the steering speed (How fast can the steering servos rotate?) and the wheelbase (What is the distance from one wheel to the other?).

A slow approach can come closer to the obstacle until the maneuver has to start than a fast approach. A slow approach can turn full 35° right when the treshold is reached, the fast approach and a 35° angle could let the Grasshopper loose grip and slide into the obstacle or worse and flip over.

Do you have in mind to measure the current speed and make it accessible from the Arduino? If so we could elaborate more here and maybe introduce some mathematics into the final algorithm.

Wow did I miss something? Since when do we prove stuff with mathematics here on LMR?^^ Just kidding. It sounds like a very good idea to change the threshold relative to the speed.

Regarding measuring the speed, the RC car uses an electronic speed controller to drive the motor.  That in turn is driven by PWM signals, making it very easy to control from the Arduino; my current code treats the ESC as a servo, so that I can just use servoname.write(value); to set the speed.  Because the servo range is 0-180, I've calibrated the ESC so that 90 is the neutral position, below 90 is reverse motion and above 90 is forward motion (the ESC has a built in calibration function to set: neutral throttle, full acceleration and full reverse).  At the moment, I only use 105 for forward motion and 75 for reverse; you can see the speed that those values translate to in this video.  I don't have exact measurements but for reference, this video shows the sort of speed that a stock Grasshopper can attain.  Either way, the speed is easily accessible to the Arduino code.

Stopping is a little awkward.  Ideally you need to use the ESC's braking functionality, rather than setting the speed to neutral.  If you set the speed to neutral, it will just continue rolling forward; however if you engage the brakes - by going from forward motion straight to reverse - then the motor locks and stops the car dead (or as dead as possible - if you're going fast, it'll usually skid).  I will probably set up an absolute minimum distance for the front sensor and tie this to an interrupt; if therefore it reaches that threshold it will stop dead and reverse away rather than decelerate.  I've currently employed this behaviour with the non-ranging IR sensors.

The stock configuration is pretty maneuverable; it can corner at top speed with the steering fully locked without any problem at all.  I put it through quite a lot of punishment before it's conversion to autonomy and even on rough ground a sharp turn would just tend to skid slightly, rather than flipping.  


You are correct in that the current speed of the car is going to affect the steering sensor thresholds.  To be honest, I haven't given much consideration to that at this point, other than storing the thresholds in variables.  What I meant by recalibrating lookup tables is having, for example, a pre-defined set of steering values and then needing a different set of values for a higher speed.  I have a fair idea of the turning circle in relation to speed already - at top speed I probably don't need more than 10-15cm for a full-lock turn to avoid an obstacle to the side (possibly more to account for possible skids) - but I may well switch back to normal R/C control to do some more empirical measurements as I ramp the speed up.  I'm going to stick to relatively slow speeds for now, until I get the principles nailed - baby steps, and all that ;).

int detectionthreshold = 35;

int rangeofservomotion = 35

steeringservoposition = neutral + ((detectionthreshold - leftsensorreading) * (rangeofservomotion / detectionthreshold));

lulz...sorry I have nothing constructive, just wanted to point this out. I see what your trying to do though and I think it's a good idea. 


if either one of those variables were to change for whatever reason, his calculation would be just as valid and have a purpose.