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

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

my code :GitHub - panagelak/4WD-drive-arduino-code-with-rosserial-encoders-pid: arduino due code to control a 4 wheeled differential vehicle by a cmd_vel callback using rosserial pid_arduino_library and quadrature_encoder library

depedencies : rosserial pid arduino library GitHub - zacsketches/Encoder: Arduino quadrature encoder library. Implements a simple to use, interrupt driven quadrature decoder for Arduino.

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

Also i cant seem to easily implement auto tune since the velocity of each wheel depends on the other
Also the wheels dont start sometimes all together

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 or 2WD vehicles with encoders and ros without bying extra encoder chips All you need is an arduino due and 2 L298N motor controllers

Thanks for your time!!