getting wrong gains for dc motor pid

[PrathamMeena]

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
byte ATuneModeRemember=2;
double kp=0.43793,ki=0.06332,kd=0.75718; // parameters obtained from autotuner
double aTuneStep=45, aTuneNoise=10, aTuneStartValue=55;
unsigned int aTuneLookBack=1;
unsigned long modelTime, serialTime;
double kpmodel=1.5, taup=100, theta[50];
double outputStart=5;
boolean tuning = false; //changed to false after tuning

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

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

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.