Skip to content

getting wrong gains for dc motor pid  #22

@PrathamMeena

Description

@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.

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions