4WD vehicle arduino rosserial, pid, quadrature, encoder help

edit

Hi, i have build a 4WD vehicle with rosserial controlled by a cmd vel topic the arduino due code uses the pid arduino library and quadrature encoder library along rosserial

my code : https://github.com/panagelak/4WD-driv...

depedencies : rosserial https://playground.arduino.cc/Code/PI... https://github.com/zacsketches/Encoder

My problem is that if i rotate the robot and then stop it either with 'k' button or let it stop and then try to move forward the robot tries to Rotate as if the ''the pid is still active'' i cant seem to be able to reset the pid when the robot stops

I use the teleop node since the default teleop_twist_keyboard did some weird behaviour on my robot

Any advice on improving the robustness of my code welcomed

This code i hope can be used as a template to cheaply and easily control 4WD vehicles with encoders without bying extra encoder chips All you need is an arduino due and 2 L298N motor controllers

Thanks for your time!!