Need help with a steering algorithm...
April 13, 2012
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.