The Sensor, mount and Servo
From the figure below, you see exactly how i mounted the sensor to the mount and thus to the servo, this allows me to program the ultrasonic range sensor to scan 165 °, taking readings at 0 ° (the right distance), at 90 ° (the center distance) and at 165 ° (the left distance). Why 165 you ask? Well i left some room to accommodate the error in servo (keep in mind you have to experiment to find your value). What happens here is, as the robot navigate, the sensor is at 90 °, now whenever there is an obstacle in >=40cm, the bot stops, scans around taking a reading of both left and right and then makes a decision to take the path (left or right) with the highest distance to another obstacle. Okay okay am just going to give you the code.cm = get_distance(); motor_speed = get_motor_speed(cm); type = bot_move(motor_speed, cm); if( type == 1){ servo2.write(0); delay(500); right = get_distance(); //scan to the right delay(500); servo2.write(165); delay(600); left = get_distance(); //scan to the left delay(500); servo2.write(90); //return to center delay(100); compareDistance(); }What you see here is the loop, we get the distance from the sensor with the
get_distance()function.
int get_distance(){ // establish variables for duration of the ping, // and the distance result in inches and centimeters: long duration, cm; digitalWrite(trig, LOW); delayMicroseconds(5); digitalWrite(trig, HIGH); delayMicroseconds(15); digitalWrite(trig, LOW); duration = pulseIn(echo, HIGH); cm = microseconds_to_centimeters(duration); //Converting distance to cm return cm; }Based on that (left and right), the distance is read by the function
compareDistance()which makes the decision on which path to take next.
void compareDistance(){ if (left>right) //if left is less obstructed { turn_left(); delay(500); } else if (right>left) //if right is less obstructed { turn_right(); delay(500); } else //if they are equally obstructed { turn_right(); delay(1000); } }That easy right?
Well you have seen most of the rest of the code in my previous posts, however i have restructured some movements, like for instance the function calls to left, right, back and forward in specialized function calls. Am just gonna give it to you and you figure out the rest. But just for argument sake, here are some specialised function calls you can find in the code:
get_motor_speed()
function just takes in a distance as argument and usesmap()
to find a suitable speed for the bot given the distance, setting it to the global motor_speed variable.set_speed()
function takes in the motorspeed as argument and sets it to each of the motors (front left and right, back left and right).bot_move()
function on the other hand takes both the distance and speed based on the previous functions, decides how should the bot move given this variables, it also sets the speed of the motors.
Current AuRGent Structure
I think i can say we covered the most important aspects of this issue, if you need the full code kindly go to my repo GitHub and the video for this feature will be made available momentarily on my channel YouTubeComing soon: Upgrades
- FSM (Finite State Machine): So honestly i hate my implementation with the bunch of if-then-else, on my defense, this solution was a preliminary hack solution. However in the next issue on this project, i will introduce FSM and a refactored version of my code which will also be in the repo.
- Reinforcement Learning: I am still designing my policy matrix to enable my bot to learn from experience and come up with its own optimal policy instead of my approximate policy in the current implementation.
- Task and path finding: I will also implement using a GPS and Compass module a feature to enable navigation from a given state to another with (as much as i can) optimal solution.