getting wrong gains for dc motor pid

Hi,
I’m using the autotune PID library for finding the gains of the dc motors of my differential drive. I am using a teensy 3.2 microcontroller, a sabertooth 2x12 motor driver, HC-05 Bluetooth module, two dc motors, and two external rotary/incremental encoders having 360ppr. I was unable to find any example to use the autotune library except for the library example so I used it as a reference and made the changes to suit my code. Here it is

#include <PID_v1.h>
#include <PID_AutoTune_v0.h>
#include <SoftwareSerial.h>
#include <SabertoothSimplified.h> //library for sabertooth motordriver

SoftwareSerial SWSerial1(0,2);
SabertoothSimplified ST1(SWSerial1);

int pinA1 = 11;
int pinB1 = 12;
int pinA2 = 7;
int pinB2 = 8;

int count1 = 0, count2 = 0; // encoder counts
int prevCount1=0, prevCount2 = 0; // last count

unsigned long prevTime1=0,prevTime2=0,prevTime3=0,prevTime4=0,prevTime5=0;

double reqRPM1 = 150,reqRPM2 = 150,reqTheta = 0 ; // required RPM
double actRPM1 =0,actRPM2= 0,motorInput2=0; // actual RPM
double kp=0.43793,ki=0.06332,kd=0.75718; // parameters obtained from autotuner
double aTuneStep=45, aTuneNoise=10, aTuneStartValue=55;
double kpmodel=1.5, taup=100, theta[50];
double outputStart=5;

byte ATuneModeRemember=2;

unsigned int aTuneLookBack=1;
unsigned long modelTime, serialTime;

boolean tuning = false; //changed to false after tuning
boolean useSimulation = true; //set to false to connect to the real world

PID myPID(&actRPM2, &motorInput2, &reqRPM2,kp,ki,kd, DIRECT);
PID_ATune aTune(&actRPM2, &motorInput2);

void Check2() // interrupt function to get encoder readings
{
  if(digitalRead(pinB2) == digitalRead(pinA2))
  count2--;
  else
  count2++;
}

void setup()
{
  myPID.SetMode(AUTOMATIC);
  aTune.SetControlType(1);
  ST1.motor(1,0); // running only one motor for now
  delay(1000);

// if(tuning) //commented this code otherwise the autotuner was not starting
// {
// tuning=false;
// changeAutoTune(); //this functions sets output to atuneStartValue and was not letting
//autotuner to start, had to comment this for it to run
// tuning=true;
// }

  serialTime = 0;
  if(useSimulation)
{
  for(byte i=0;i<50;i++)
   {
     theta[i]=outputStart;
   }
  modelTime = 0;
}
//encoder pins
  pinMode(pinA2,INPUT);
  pinMode(pinB2,INPUT);
//interrupt
  attachInterrupt(digitalPinToInterrupt(pinA2),Check2,RISING);
//keeping track of time from start
  prevTime1 = micros();
  prevTime4 = micros();
  prevTime5 = micros();

  Serial.begin(9600);
  SWSerial1.begin(9600);//serial for sabertooth motor driver
}

void loop()
{
  unsigned long now = micros();
  ST1.motor(1,0); // running only one motor

  if((micros()-prevTime1)>10000) // run every 10 ms
{
  if(tuning)
   {
     byte val = (aTune.Runtime());
     if (val!=0)
      {
        tuning = false;
      }
     if(!tuning)
     { //we're done, set the tuning parameters
        kp = aTune.GetKp();
        ki = aTune.GetKi(); 
        kd = aTune.GetKd();
        myPID.SetTunings(kp,ki,kd);
        AutoTuneHelper(false);
     }
}
else {
  myPID.Compute();
}

  if(useSimulation)
{
   theta[30]=motorInput2;
   if(now>=modelTime)
{
   modelTime +=100;
   actRPM2 = getRPM2(); // instead of DoModel() in order to update input value I placed my
// code to update input in this portion
   ST1.motor(2 ,motorInput2); // sending values to motor
}
}

if(micros()>serialTime)
{
  SerialReceive();
  SerialSend();
  serialTime+=500;
} 
  prevTime1 = micros();
}
}

  int getRPM2() // calculating RPM
{
  long pps2 = long(count2 - prevCount2);
  float ppr2 = 360;
  prevTime4 = micros() - prevTime5;
  double tt = prevTime4 / 6000.0 ;
  double actRPM2 = ((pps2*10000.0)/(tt*ppr2));
  prevTime5 = micros();
// SWSerial2.print(" rpm2= " );
// SWSerial2.println(actRPM2);  
  prevCount2 = count2;
  return actRPM2;
}

void changeAutoTune()
{
  if(!tuning)
{
  //Set the output to the desired starting frequency.
  //removed one line of code that sets the output to aTuneStartValue
  aTune.SetNoiseBand(aTuneNoise);
  aTune.SetOutputStep(aTuneStep);
  aTune.SetLookbackSec((int)aTuneLookBack);
  AutoTuneHelper(true);
  tuning = true;
}
else
{ //cancel autotune 
  aTune.Cancel();
  tuning = false;
  AutoTuneHelper(false);
  Serial.print("Adad222");
}
}

void AutoTuneHelper(boolean start)
{
  if(start)
  ATuneModeRemember = myPID.GetMode();
else
  myPID.SetMode(ATuneModeRemember);
}

void SerialSend()
{
  Serial.print("setpoint: ");Serial.print(reqRPM2); Serial.print(" ");
  Serial.print("input: ");Serial.print(actRPM2); Serial.print(" ");
  Serial.print("output: ");Serial.print(motorInput2); Serial.print(" ");
  if(tuning){
  Serial.println("tuning mode");
} else {
  Serial.print("kp: ");Serial.print(myPID.GetKp(),5);Serial.print(" ");
  Serial.print("ki: ");Serial.print(myPID.GetKi(),5);Serial.print(" ");
  Serial.print("kd: ");Serial.print(myPID.GetKd(),5);Serial.println();
}
}

void SerialReceive()
{
  if(Serial.available())
{
  char b = Serial.read();
  Serial.flush();
if((b=='1' && !tuning) || (b!='1' && tuning))changeAutoTune();
}
}

Even after I commented the changeAutoTune and AutoTuneHelper the autotuner was taking forever to end. So I looked at the library .cpp file and tweaked with LookBackSec, Noise, and some other values, getting no success. Finally, when I reduced the value of nLookBack in RunTime function to a value lower than 9 (worked good till 5 for me) the tuner came to an end after oscillating for a while. But the problem is the values that I am getting are too high that it makes the pid unstable.

These are some of the values I recieved from various iterations:

Parameters of motor1 Parametes of motor2

kp: 0.43793 ki: 0.18999 kd: 0.25236 kp: 0.24125 ki: 0.08561 kd: 0.16996
kp: 0.36965 ki: 0.12025 kd: 0.28408 kp: 0.43793 ki: 0.21373 kd: 0.22433
kp: 0.36965 ki: 0.20616 kd: 0.16570 kp: 0.43793 ki: 0.07124 kd: 0.67299
kp: 0.36965 ki: 0.09019 kd: 0.37875 kp: 0.43793 ki: 0.06332 kd: 0.75718
kp: 0.44646 ki: 0.09042 kd: 0.55110 kp: 0.43793 ki: 0.06106 kd: 0.78521
kp: 0.48419 ki: 0.14538 kd: 0.40315 kp: 0.43793 ki: 0.11397 kd: 0.42069
kp: 0.43793 ki: 0.15543 kd: 0.30847
kp: 0.43793 ki: 0.17097 kd: 0.28044

SoftwareSerial SWSerial1(0,2);

Pin 0 is one of the hardware serial pins. You should NOT be doing software serial on that pin.

void SerialReceive()
{
  if(Serial.available())
{
  char b = Serial.read();
  Serial.flush();
if((b=='1' && !tuning) || (b!='1' && tuning))changeAutoTune();
}
}

You clearly have no clue about properly indenting your code, OR what flush() does.

Sorry for the mistake, since I am not receiving any data through SWSerial1 I didn't think of it much and assigned 0 to it. Thanks for the advice.

I'll work on my indentation and as I said, I couldn't find any other reference to using the PID library I just changed the code to my needs. I started this month only so don't have much idea about things for now.

It would be great if you could help me work out my issue. I have to drive my three wheel differential straight for which I am trying to make the wheels rotate at constant RPM, but I am not getting a stable result. I tried manual tuning but that didn't work out well either so I turned to autotuning. Any suggestion would be helpful.