This may be more of a general programming question than arduino specific; I guess the principles apply to all programming languages. I have a Tamiya Grasshopper R/C car which I've converted into an autonomous vehicle (or robot, if you prefer). I'm having a bit of trouble with programming the reaction to the sensors... 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 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 can't write a float to a servo). I'm also ignoring forward/reverse motion at this point, 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. Code: //take reading from left sensor if (leftsensorreading < detectionthreshold) { while (leftsensorreading < detectionthreshold) { steeringservoposition = neutral + ((detectionthreshold - leftsensorreading) * (rangeofservomotion / detectionthreshold)); //take new reading from left sensor } 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 RC car and they work just fine. Is there a better way of doing this, or have I c*cked 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.
That's ok, i'm not sure why it would get stuck in a loop, but I think there is a fundamental flaw in this approach if the whole thing is written in this manor. This approach means that it's only ever thinking of one thing at once on one parameter. What it should do is examine all parameters every loop then make a decision based on the readings, so there should only really ever be a single loop ideally (with a few exceptions for mathy things and the like).
Hi. I know very little about Arduino and it is late, so I'll probably make a fool of myself. I was just thinking of one function that might be handy for this (although you probably already know it); the map funtion (http://arduino.cc/en/Reference/map). For example: Code: while (leftsensorreading < detectionthreshold) { steeringservoposition = map(leftsensorreading, 50, 0, 91, 125); //take new reading from left sensor } Or something similar. Those numbers are just examples of course, but if my thinking is correct, when the sensor reads 50 (ie. max distance which still falls within the threshold), the servo would be commanded to go to 91 degrees (ie. minimum deflection). By the time the sensor reads a distance of 0, the servo would be at maximum deflection. You could modify it so that max deflection occurs before 0. I don't know, if I'm talking stupid just ignore me Anyways, I can't see any reason why your loop should fail to terminate.
Cheers for the advice, both. The plan (and current config with the IR sensors) that it takes a reading from all sensors first, checks which one has detected an object and then take the appropriate action; I don't plan to put loops within void loop(), but have everything else in its own procedure. I'm not sure I can avoid for/while loops, because I don't want to use delays. I don't want it to steer straight again until it has cleared the obstacle... I posted the same question on a robotics forum, and the map() function was suggested there too (although the poster referred to autoscale(), which has been incorporated into the code Arduino library as map()). It seems like this may be a better way to go. I wasn't actually aware of it before ; my programming background is very much functional - i.e. scripting & macros - rather than theoretical or mathematical. Hence why I tend to fall flat on my face when it comes to mathematical algorithms .
I found the reason while testing at the weekend: one of the damn sensor pins kept getting unplugged from the Arduino board while connecting/disconnecting the Arduino's USB cable...
Haha, I've had a few incidents like that myself Glad you found it anyways, good luck with the project
I'm a bit late here, but I can advise you a bit on a potential problem I see. As it stands, it looks like you're only taking information from 1 sensor at a time and adjusting steering based on that 1 sensor (then I'm assuming you move on to poll the other sensors). This might be a problem if there's a lot of junk around that's triggering several sensors at a time. Rather than having 1 algorithm fire for each sensor, I would suggest developing a function that calculates the angle to steer at based on input from all your sensors. So f(S0, S1, S2, S3) -> degrees. I'm not sure how you'd do this since I don't know what kind of data you're sensors are returning and what kind of turn you'll want for what situation, but it should be possible to develop a function for what you want to do. My suggestion for developing the function would be to list out scenarios that can happen with your sensors (all sensors say no obstacles, sensor0 says obstacle at 50cm and the rest say no obstacles, etc.) and just drop in the turn value that you think is appropriate. Then put that table into something like Graphical Analysis and see if you can create a workable trend function that hits all the points you mapped and accounts for in-between values accurately. If you do the function correctly, I believe you would have a much smoother object evasion system. The problem with doing a system this way is that if you want to tweak the turning mechanism you're basically going to have to recalculate your function all over again. It's a lot more work to create and maintain, but I think that work would be worth it.
Many thanks for the input. I did do some further testing on the weekend but I was thinking about this in work yesterday, specifically what you mention regarding accounting for all sensor inputs at once. You are correct in that my original approach was to take the two sensor readings and then have separate blocks of code for each sensor reading. I can indeed see the problem with this: if there are obstructions on either side, it will always turn in one direction - this direction will be determined by whichever block of code is listed first. For reference, the sensors measure the distance to an obstacle in milliseconds: how many milliseconds have elapsed between sending a pulse and receiving an echo. The library for the sensor has some functions which take a reading and then convert milliseconds into centimetres or inches; I currently use the functions which return a value in CM. After a bit of headscratching in work, 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. Code: 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. 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...
Looks fine to me, all I would recommend would be to put the steering function into its own function and just call that. It'd look a lot cleaner and would probably be easier for you to find and edit later.
