Need help with a steering algorithm

Community General Discussions
, , , ,
1

 

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.

2

I don’t know arduino well, but, this might get you started.

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.

3

Many thanks

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.

4

I didn’t find out about map until I just read a new forum post.

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 );

use:

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.

5

Try finding a multiplier

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 :slight_smile:

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);
    servo.write(servoAngle)
}

6

Treshold Relative to the Current Speed

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.

7

int detectionthreshold =

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. 

 

8

I saw that it would divide to 1, but,

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

9

Instead of map or autoscale

Instead of map or autoscale you can do this: 

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

10

Wow did I miss something?
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.

11

Wow…

Rather overwhelmed by the responses; I wasn’t expecting so many people to reply!  It seems like I had the right sort of idea, but maybe with a bad implementation.

Thanks for all the advice; I’ll take a look at the code samples/suggestions posted and do some testing.

12

Indeed

That was the idea; the threshold was only set to 35 for testing, and to make the maths easier.

13

Good points.

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 ;).

14

Ok, was thinking about this in work…

At the weekend I tested out some of the examples here, as well as some new ideas of my own, but I wasn’t completely happy with the results.  I’m certainly a lot further along though.

I was thinking about this in work earlier (I sometimes get very bored in work…) and the problem is in my approach: the way I’m currently doing this is that the two sensors are read and then there are two separate loops/if statements for each sensor reading.  This means that if there are obstacles either side - both within the detection threshold - then whichever block of code is listed first is going to get executed first.  This means that it’ll always turn one way in this scenario.  This isn’t exactly ideal behaviour; a far better way would be to come up with a function/calculation that accounts for sensor on both sides and give one steering value.

So, after a bit of headscratching, and some testing & prototyping in MS Excel, I managed to come up with the following - I’ll admit, this looks a lot more complex than the Excel formula…  By the way, the command button in the spreadsheet I linked to is used to recalibrate the ranges on the scrollbars - you need to do this if you adjust the min/max detection thresholds, and this is why it needs macros/security enabled.

 

 

int intMinAvoid = 5;

int intMaxAvoid = 50;

int intServoRange = 35;

int intNeutral = 90;

int intLeftSensorReading;

int intRightSensorReading;

int intSteeringPosition;

int intOffsetValue;

float fltOffsetValue;

 

intLeftSensorReading = ReadUltrasonicSensor(Left);

intRightSensorReading = ReadUltrasonicSensor(Right);

 

while (intLeftSensorReading < intMaxAvoid || intRightSensorReading < intMaxAvoid)

{

 fltOffsetValue = (((intMaxAvoid - intLeftSensorReading)/(intMaxAvoid - intMinAvoid)) * intServoRange) - (((intMaxAvoid - intRightSensorReading)/(intMaxAvoid - intMinAvoid)) * intServoRange));

 

 intOffsetValue = (int) fltOffsetValue;

 intNewSteeringValue = intNeutral + intOffsetValue;

 SteeringServo.write(intNewSteeringValue);

 intLeftSensorReading = ReadUltrasonicSensor(Left);

 intRightSensorReading = ReadUltrasonicSensor(Right);

}

 

 

So. If this detects an object in range on either side, it carries out these calculation steps: deduct the current reading from the maximum detection threshold; deduct the minimum detection threshold from the maximum detection threshold; now divide the first value by the second.  The result of this is a percentage; multiply this value by the maximum range of the servo movement.  This works out how far away from neutral the servo has to move in order to avoid the object.  Because I want to account for for objects on either side however, those calculations are performed for each sensor.  E.g.: if an object is 21cm away on the left and 12cm away on the right, the calculated servo offsets would be 22 to the right and 29 to the left.  The penultimate step is to deduct the output of the calculation for the right sensor from the calculation for the left sensor, and finally add this value to the value 90 (which is the neutral position for the steering).  With the distances I mentioned in the previous example, the steering value would be 83 - a slight steer off to the left, but not too far, as there is still an obstruction off to the left.

Honestly, it’s probably far easier to have a look at the Excel prototype I linked to, rather than trying to follow my explanation - I confused myself several times whilst writing it.  I haven’t yet tried this out in the Arduino IDE so the code I posted up there may not be 100% syntactically correct, but the mathematics should stand.

 

Am I overcomplicating this, or am thinking along the right lines?

Lastly, I think that modelling a steering algorithm for a robotics hobby in Excel while I’m in work has taken me to a new level of nerdiness, and I’m pretty sure I’ve earned some kind of badge for it… :wink: