PID controler in arduino

edit

hi i am trying to move the robot with PID code implement in the arduino code and drive the robot by teleop twisted now i have problem in response i use rosserial as interface between ros and arduino. loop time in the pid code is 100ms that mean its 10hz as frequency and i publish the encoder data at 10 hz but while i am changing the direction or move for left to forward there is some lake of response i tryed to play with the parameter but not help is there any way that i can use PID in ROS and just controll the movement of the robot by telope twisted without and lake of response just move it around and speed controled by ros not arduino . i use arduino due and ros indigo this is the PID code in arduino :

    int updatePid(int id, int command, double targetValue, double currentValue) {
      double pidTerm = 0;                            // PID correction
      double error = 0;
      double new_pwm = 0;
      double new_cmd = 0;
      static double last_error1 = 0;
      static double last_error2 = 0;
      static double int_error1 = 0;
      static double int_error2 = 0;

      error = targetValue-currentValue;
      if (id == 1) {
        int_error1 += error;
        pidTerm = Kp*error + Kd*(error-last_error1) + Ki*int_error1;
        last_error1 = error;
      }
      else {
        int_error2 += error;
        pidTerm = Kp*error + Kd*(error-last_error2) + Ki*int_error2;
        last_error2 = error;
      }

  return constrain(double(command)+ pidTerm,-120, 120);

i toke this code from : https://sungjik.wordpress.com/ now this code work for him with just proportional gain for me there is a lake of response if i use just proportional gain so i use Kp and Ki its better but still have some problem . i am using 2 h-bridge IBT-4 arduino i hope all of this information can help you to give some answer. thanks.

edit: here is the code that give the speed to motor:

if(time-lastMilli>= LOOPTIME)   {      // enter tmed loop
    getMotorData(time-lastMilli);
    PWM_val1 = updatePid(1, PWM_val1, r1, rpm_act1);
    PWM_val2 = updatePid(2, PWM_val2, r2, rpm_act2);

 if (PWM_val2> 0){
    analogWrite(motor1_pin1,abs(PWM_val2));
  analogWrite(motor1_pin2,0);
  }
  if (PWM_val2<0) {

  analogWrite(motor1_pin1,0);
  analogWrite(motor1_pin2,abs(PWM_val2));
}
  if (r2==0) {

  analogWrite(motor1_pin1,0);
  analogWrite(motor1_pin2,0);
}
  if (PWM_val1>0) {
    analogWrite(motor2_pin1,abs(PWM_val1));
    analogWrite(motor2_pin2,0);
  }
if (PWM_val1<0) {
analogWrite(motor2_pin1,0);
analogWrite(motor2_pin2,abs(PWM_val1));
}
if (r1==0) {
  analogWrite(motor2_pin1,0);
analogWrite(motor2_pin2,0);
}



    publishRPM(time-lastMilli);
    lastMilli = time;
  }
  if(time-lastMilliPub >= LOOPTIME) {
  //  publishRPM(time-lastMilliPub);
    lastMilliPub = time;
  }

}

now if return an int to the PWM and used kp and ki gain on this case the motor will move forward without any change in direction but if i change PWM to r1 or r2 while r1 and r2 is the subscribe value from ROS it will move in all direction without any problem and with very high response i hope any one can help me to solve this problem how i can git the same response of the last case with the accuracy that i had when i return a double not int.