The steering system you have chosen is not optimal for PID steering.

It is much easier to just control the speed (power) to the motors individually and differentially.

Example: the speed of MotorL is set to (base speed - correction) and MotorR is set to (base speed + correction), where correction = Kp*(heading error). Later add Kd and Ki if needed.

If the heading error is positive, correction is positive, MotorL slows down and MotorR speeds up, and the model veers to reduce the heading error. Vice versa for heading error negative.

Pay close attention to compass wrap when calculating heading error. Here is one way to do it:

`// routine to calculate heading error in degrees, taking into account compass wrap`

int heading_error(int bearing, int current_heading)

{

int error = current_heading - bearing;

if (error > 180) error -= 360;

if (error < -180) error += 360;

return error;

}