issue with teleop and pid with differential drive !?

edit

hello guys,

i have

• differential drive robot with castor for support,rplidar,rotarary encoder and brick imu
• using rosserial Arduino

i want to do mapping with this and choosed gmapping

The main loop in the motor_controller.ino converts encoder tick counts into the current rpm of both motors and controls the speed of the motors using PID control. The rosserial library to communicate with the rest of the ros nodes in the pc via USB. The program subscribes to the cmd_vel topic which sets the desired speed of the motors and publishes the actual speeds on the rpm topic

while tuning navigation teleop speed is very low and still after 2 seconds it takes full speed in a different direction,

I'm confused is it because my left and right having separate motor PWM values or still PID tuning is required?

I thought of using pid ros pacakge but will it work with my architecture and also I don't know how to use it?

here is my code :

#if (ARDUINO >= 100)
#include <Arduino.h>
#else
#include <WProgram.h>
#endif
#include <ros.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/Twist.h> //telop
#include <std_msgs/Int32.h> //sending encoder data
#include <ros/time.h> //to get current ruuning ros time  frame
#include <Encoder.h> // encoder 


#define RH_Directionf 26  //input 1
#define RH_Directionb 28  // 2
#define LH_Directionf 22  //in 1
#define LH_Directionb 24  //in 2
#define RH_Pwm 9  // motor 1 pwm
#define LH_Pwm 10 //pwm pin for motor 2
#define LOOPTIME        100   // PID loop time(ms) calling loop at fixed for pid 
#define SMOOTH      10  //not used

// variables to store the number of encoder pulses
// for each motor

Encoder myEnc1(2, 4);   //encoder declarations
Encoder myEnc2(3, 5);
long oldPosition1  = 0, newPosition1; //variable to store enc data from encoder library
long oldPosition2  = 0, newPosition2;
long d1r, d2r = -999, d1l, d2l = -999;


volatile double leftCount = 0; //not used
volatile double rightCount = 0;//n u
volatile double prev, now , start, time_delay = 0, counter = 0, prev_rotR, prev_rotL, cur_rotR, cur_rotL; //variables for pid ,getting rps and rpm
volatile double error_l = 0, last_error1_l = 0 ;
volatile double error_r = 0, last_error1_r = 0 ;
volatile double int_errorl;
volatile double int_errorr;

unsigned long lastMilli = 0;       // loop timing
unsigned long lastMilliPub = 0;
double Required_rpml = 0;
double Required_rpmr = 0;
double RPM_left = 0;
double RPM_right = 0;
double Required_rpml_smoothed = 0;
double Required_rpmr_smoothed = 0;
char direction1 ;
char direction2 ;
int PWM_val1 = 0;
int PWM_val2 = 0;
float Kp = 0.5;
float Kd = 0;
float Ki = 0;

    ros::NodeHandle nh;

void handle_cmd( const geometry_msgs::Twist& cmd_msg)
{
  double x = cmd_msg.linear.x;
  double z = cmd_msg.angular.z;
  if (z == 0) {     // go straight
    // convert m/s to rpm
    Required_rpml = x * 95.469296598;
    Required_rpmr = Required_rpml;
  }
  else if (x == 0) {
    // convert rad/s to rpm
    Required_rpmr = z * 33.90000294; ///z < .307
    Required_rpml = -Required_rpmr;
  }
  else {
    Required_rpml = (x * 95.469296598) - (z * 33.90000294);
    Required_rpmr = (x * 95.469296598) + (z * 33.90000294);
  }
  int_errorr = 0; //error for pid
  int_errorl = 0;
}

ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", handle_cmd);
geometry_msgs::Vector3Stamped rpm_msg;
std_msgs::Int32 rot, enc1;
ros::Publisher rpm_pub("rpm ...