I am glad to hear that your sensor array is working as expected!
Helping with user code is typically beyond the scope of our technical support, but I took a quick look at your code and can give you a few general suggestions for troubleshooting line following programs like this.
One error I noticed in your program is that you are expecting line position values from the readLineBlack() function to range from 0 to 7000, and 3500 to correspond to the middle of the sensor array. However, since you are only using 6 sensors, the range will actually be 0 to 5000, and 2500 will correspond to the middle of the array (or at least, the part of the array you are using).
If you continue having trouble after that fix, then the issue probably has to do with your PID tuning. A quick internet search (or even just searching our forum; I particularly like the discussion in this thread) will give you up a plethora of advice on this topic, but here is the procedure I would follow:
- Write and test a simple program to confirm that your motors are turning the correct directions and have similar speed performance.
- Simplify the control algorithm by setting KD to 0 so you can focus on KP first.
- Comment out the
set_motors()command in your main loop, and add some serial print lines to show what is going on. Some good values might includeposition,error,leftMotorSpeed, andrightMotorSpeed. - Since
set_motors()is commented out, test this program by just manually moving your robot over the line, and see how the values respond. Use that feedback to adjust the KP value as seems needed. - When you think you have it working well without motors, comment the
set_motors()line back in. Your goal with the KP term is to find a small value that causes your robot to steadily oscillate back and forth across the line while it goes around the course. I strongly recommend when you first start, reduce your minimum and maximum speed values significantly (e.g. 1/4 what they are now, or maybe even slower). It will be much easier to troubleshoot and tune a slow robot, than a fast one, and though you might need to retune when you increase the speed, you will have something you know works to fall back on. - Once you think you have a good KP value, add in the KD value to try to smooth out the robot’s motion. It might be helpful to follow a similar procedure of commenting out the
set_motors()command, and printing values to see how the KD term affects them before adding the motors back in. - Once you think you have good KP and KD values for running your robot at a slow speed, incrementally increase your minimum and maximum speed values, retuning KP and KD as needed.
- Patrick
