Arduino Wall Follower Patrolling Over Room - Using PID Controller

For this small project, I have adopted the basic concept of PID controller from the below article
http://www.inpharmix.com/jps/PID_Controller_For_Lego_Mindstorms_Robots.html

I applied this concept (for Lego Mindstorms Robot), to Arduino robot. I applied the "Line follower" formula given in the above article, to "wall follower". As per the above site, the "Line Follower" formula is:
   error = LightValue - offset        ! calculate the error by subtracting the offset
   integral = integral + error        ! calculate the integral
   derivative = error - lastError     ! calculate the derivative
   Turn = Kp*error + Ki*integral + Kd*derivative  ! the "P term" the "I term" and the "D term"
 
In case of Wall Follower, "offset" is the setpoint (The constant distance which we desire to keep between wall and robot). The LightValue (the instantaneous light sensor readings) is substituted by instantaneous ultrasonic sensor readings. The "Turn" variable can return the pwm value for arduino to vary the speed of the motor. But in my case, I am using "Adafruit Motor Shield V1" (https://learn.adafruit.com/adafruit-motor-shield/overview) which is stacked on top of arduino. This shield drives the two dc motors. I have downloaded and imported its libraries. These libraries allow us to control the speed of the motor without getting into PWM details. The speed can vary between 0 to 255. In my case, I am directly controlling the speed with the PID formula. That is

speedCorrection = Kp*error + Ki*integral + Kd*derivative;
leftWheelSpeed = 128 - speedCorrection;
rightWheelSpeed = 128 + speedCorrection;

Just for information: For the results in the below video, I am applying three PIDs. One for right turn, One for left turn and One for straight line. So I had to keep adjusting 9 parameters to tune it for patrolling the room. I began with specifying kp = 1 for straight drive. Then slowly keep on increasing or reducing kp. The robot oscillates but it follows the wall. Then I applied kd to reduce the oscillations and stabilize the robot. I have put some ki value (which is really small) just for experiment sake, but it is not causing any harm to robot. In future, there might be a possibility to calculate the PID tuning values through Neural Network or something. But I have not explored that possibility until now.

As far as my observation goes, the PID constants depend upon the geography of the surrounding area. For example, if you have 3 right and 2 left turns in the room/lab, then you have one set of PID constants. However, if you change the room where you have 2 right and 3 left turns, then you have totally different set of PID constants. The "bottom line" is, you will "have to" tune the PID for your requirements. I would suggest the following approach for tuning:

1. Just focus on getting the robot follow a straight wall first. No turns. Remove the code for right/left turns and apply it to the robot.
2. Once the straight wall is followed properly, introduce left turn. Add the left turn code.
3. Introduce right turn at the end.

Some thoughts about left turn: As per my observation, the left turn of robot totally depends on the following

• PID parameters
• The voltage
• The speed of the robot.

To reduce the speed of the robot: You can try reducing the speed at the turn. In stead of using 128 as a reference speed, use 64 as reference speed. So the equation will be

If (correction > 0 && correction > 64)
Correction = 64;
If (correction < 0 && correction < -64)
Correction = -64;

leftWheelSpeed = 64 - correction
rightWheelSpeed = 64 + correction

As per my observations: When we change the primary speed, we have to change the PID tuning parameters (Kp, Ki, Kd) also. Only then it works well. If you reduce the primary speed to half (64), try to divide Kp, Ki, Kd to half and see the changes in the movement. After observing the changes, take your own decision (regarding tuning), as per your intuition.

Voltage Variations: Regarding the "voltage", it slowly reduces, as and when you experiment with the robot. This impacts the way the robot turns.

I am sharing a video of my wall follower robot which is able to patrol all over the room successfully. It starts from the sofa and ends up near the sofa. The right turn seem to be drastic. Other than that everything seems ok. This is all using PID controller. It took nearly 500 experiments (with different PID values) over last 1.5 months to reach this stage. Each experiment took almost 10 min. This robot is very sensitive to voltage changes. If the voltage is between 5.3V to 5.6V, it works correctly. Otherwise not. I will need to design a voltage regulator to maintain constant voltage to the microcontroller and sensors. Hope you will enjoy the video.

Below is the Circuit Diagram of the Robotic Car

Please find the Wall Follower Code below.

https://gist.github.com/MindstormFan/0faab8a927a8824c9ab12d89b3ac1d05

Q & A

1. Sometimes, the robot takes a U turn instead of taking a left turn .. Why?

Answer: It is because the "correction" value overflows. When I implemented the if statement, to truncate the correction to 127 as below, the problem had vanished. I had added the following statements to resolve this issue:

if(correction > 0 && correction > 127)

correction = 127;

if(correction > 0 && correction > -127)

correction = -127;

Further to correctly follow the wall after any turn, I had inserted "initializeWall" function at appropriate places. Make sure that is added in the code properly.

Lastly, you might need to change the PID values for straight line slightly to take a left turn. As usual begin with kp, the change kd  then change ki.

2. A question about the code. My robot sees the obstacle in front of him but drives straight into it, it doesnt turn, what can cause this problem?

Answer: When my robot sees an object in front, it takes a right turn. I have put some extreme tuning parameters (like MY_Kp_Front=40 and MY_Kd_Front=20) to make the robot take a right turn. However, the same tuning parameters might not work for you, because of the "weight" of the robot and the "voltage" supplied to the robot. Here, you have to play with the turning params and come to a decision.

My robot patrols the complete room, only 30% of the time. For rest 70% of the time, it collides at one turn or the other. I am still working on this inconsistency. Meanwhile, I feel that, there has to be some "fall back mechanism" for the robot to complete the wall following. (For Example: when the robot collides at a turn, you can make it "take reverse for few seconds, change the angle and move on".)

3. Did you guess what parameter is responsible for the first move from starting position? If you dont know what I mean I will try to explain below: When I start the robot and put it in my target distance from the wall (which is 12cm) it goes nice and straight, but when I put closer or further from the wall the robot drives into it. Increasing Kd should help?

Answer: If the robot crashed into the wall or moved away from the wall (when kept little away or closer to the wall), that means error is getting amplified a lot in the beginning. Try reducing Kp (first) and increasing Kd (second) and both (third).

4.  Isint a negative number of motor speed a problem? For example if I set motor speed to "-40", will it see it as 40?

Answer: How the motor handles negative numbers, totally depends upon the implementation of its library/driver. For me, I am using Adafruit Motor Shield. It stores the speed value, in a unsigned int (0 to 256). Hence I have chosen a primary speed to be 128 and applied the PID correction on top of it. If the PID value goes more than 127, I have truncated it to be 127. Also, if the PID value is less than -127, i have truncated it to be -127. That will make "primary speed + correction"  to range from 0 to 256.

5. My robot turns left too early and he ends up stuck in the wall like this: 

Answer: You try changing the PID coefficients for Left turn. If you are following my code, I have a different set of coefficients for Left turn. Try changing the derivative coefficient (Kd) for left turn. Decrease the Kd value and see the difference. In my code, the below "Correction" equation has the PID coefficients for left turn.

//PID for left turn

int speed = 2.5 * WF_Error + 8 * WF_Derivative;

In the above equation, the Kd value is 8. Reduce it.

6. I changed from 6V (which was not enough) to 9V. After doing that I had to change speeds of course, because 255 on Maximum was too much. But after doing that my robot freaked out at left turns again. It is turning wrong when there is a short wall after obstacle. Example in the picture: 

so the red line is the path of my robot. Looks like it has no time to settle down after bigger error. I tried to change Kd for left turn, I tried to lower speed more. I have no idea how to solve this problem.

Answer: I also observed that, there is a right turn, followed by an immediate left turn. The right turn PID might be affecting left PID here. Can you try making the right turn PID values little lower (if you have made them drastically high).