I'm trying to implement a PID based line follower. The robot has 2 line sensors with ranges from 0-200, with 0 being black and 200 being white. I'm trying to follow a black line.
I'm struggling to get the robot to do anything but erratically turn and confuse itself on the spot, getting wildly out of control. I've tried adjusting the setpoint value, the operators in the equations and the kp/kd values to no avail.
Is there something wrong with the algorithm I'm using? (I'm a beginner to this so it's likely)
My code is below: