Hello. I started using PID to control my wheels DC more precisely, but I dont get exactly what I wanted. From PID, I expect to get stability in controlling speed, but I don
t get it. As my robot goes long way straight, after few seconds (5-7sec) it starts waving, gets close to wall, detects it and goes back again, wave starts growing, instead of becoming more stable and stable as It goes.
I`m using this kind of robot base (http://www.miniarduino.com/wp-content/uploads/2015/05/DIY-Motor-Smart-Robot-2WD-Car-Chassis-Kit-Speed-Encoder-Battery-Box-arduino.jpg)
Arduino Uno + AdaFruit motor shield copy and two HC-Sr04 sensors. As it looks from information that I get from debug, It should get more stable as It goes, but my robot acts differently.
So what is the problem? DC wheels difference or something else? I will post code on top, to see what my PID does. (Kp, Kd, Ki is being tested, but I cant find much difference as I change them, waving don
t go away.)
Main:
#include <AFMotor.h>
#include <NewPing.h>
#include <PID_v1.h>
#include "const.h"
#include "debug.h"
#include "sensor.h"
#include "drive.h"
void setup()
{
Serial.begin(9600);
Setpoint = 150; //mm
myPID.SetMode(AUTOMATIC);
myPID.SetSampleTime(100); //ms
myPID.SetControllerDirection(DIRECT);
// ultrasonic sensor pin configurations
pinMode(ultrasonic2TrigPin1, OUTPUT);
pinMode(utlrasonic2EchoPin1, INPUT);
pinMode(ultrasonic2TrigPin2, OUTPUT);
pinMode(utlrasonic2EchoPin2, INPUT);
}
void loop()
{
readUltrasonicSensors2(); // read and store the measured distances
readUltrasonicSensors1(); // read and store the measured distances
if(Input > 140 && Input < 160)
{
myPID.SetTunings(consKp, consKi, consKd);
myPID.SetOutputLimits(-20,20);
}
else
{
myPID.SetTunings(aggKp, aggKi, aggKd);
myPID.SetOutputLimits(-100,100);
}
myPID.Compute();
drive();
debugOutput(); // prints debugging messages to the serial console
}
void Compute()
{
if (!inAuto) return;
unsigned long now = millis();
int timeChange = (now - lastTime);
if (timeChange >= SampleTime)
{
/*Compute all the working error variables*/
double error = Setpoint - Input;
ITerm += (ki * error);
if (ITerm > outMax) ITerm = outMax;
else if (ITerm < outMin) ITerm = outMin;
double dInput = (Input - lastInput);
/*Compute PID Output*/
Output = kp * error + ITerm - kd * dInput;
/*Remember some variables for next time*/
lastInput = Input;
lastTime = now;
}
}
void SetTunings(double Kp, double Ki, double Kd)
{
if (Kp < 0 || Ki < 0 || Kd < 0) return;
double SampleTimeInSec = ((double)SampleTime) / 1000;
kp = Kp;
ki = Ki * SampleTimeInSec;
kd = Kd / SampleTimeInSec;
if (controllerDirection == REVERSE)
{
kp = (0 - kp);
ki = (0 - ki);
kd = (0 - kd);
}
}
void SetSampleTime(int NewSampleTime)
{
if (NewSampleTime > 0)
{
double ratio = (double)NewSampleTime / (double)SampleTime;
ki *= ratio;
kd /= ratio;
SampleTime = (unsigned long)NewSampleTime;
}
}
void SetOutputLimits(double Min, double Max)
{
if (Min > Max) return;
outMin = Min;
outMax = Max;
if (Output > outMax) Output = outMax;
else if (Output < outMin) Output = outMin;
if (ITerm > outMax) ITerm = outMax;
else if (ITerm < outMin) ITerm = outMin;
}
void SetMode(int Mode)
{
bool newAuto = (Mode == AUTOMATIC);
if (newAuto == !inAuto)
{ /*we just went from manual to auto*/
Initialize();
}
inAuto = newAuto;
}
void Initialize()
{
lastInput = Input;
ITerm = Output;
if (ITerm > outMax) ITerm = outMax;
else if (ITerm < outMin) ITerm = outMin;
}
void SetControllerDirection(int Direction)
{
controllerDirection = Direction;
}
Variables:
/*working variables*/
unsigned long lastTime;
double Input, Output, Setpoint;
double ITerm, lastInput;
double consKp=0.05, consKi=0, consKd=0;
double aggKp=1, aggKi=0, aggKd=0;
double Kp, Ki, Kd;
double kp, ki, kd;
double outMin, outMax;
double error;
int SampleTime = 100; //0,1 sec
bool inAuto = false;
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
#define MANUAL 0
#define AUTOMATIC 1
#define DIRECT 0
#define REVERSE 1
int controllerDirection = DIRECT;
//Motor set
AF_DCMotor motor1(1, MOTOR12_8KHZ); // create motor #2, 1KHz pwm
AF_DCMotor motor2(2, MOTOR12_8KHZ); // create motor #2, 1KHz pwm
int Lmotor;
int Rmotor;
const int motorNeutral = 160;
int speedmax = 255;
// specify the trig & echo pins used for the ultrasonic sensors
const int ultrasonic2TrigPin1 = A0;
const int utlrasonic2EchoPin1 = A1;
const int ultrasonic2TrigPin2 = A2;
const int utlrasonic2EchoPin2 = A3;
#define MAX_DISTANCE 500
NewPing DistanceSensor1(ultrasonic2TrigPin1, utlrasonic2EchoPin1, MAX_DISTANCE);
NewPing DistanceSensor2(ultrasonic2TrigPin2, utlrasonic2EchoPin2, MAX_DISTANCE);
int Front;
int ultrasonic2Duration1;
int ultrasonic2Duration2;
Debug output:
void debugOutput()
{
Serial.print("Lmotor: ");
Serial.print(Lmotor);
Serial.println();
Serial.print("Rmotor: ");
Serial.print(Rmotor);
Serial.println();
Serial.print("Front: ");
Serial.print(Front);
Serial.println();
Serial.print("Input: ");
Serial.print(Input);
Serial.println();
Serial.print("Output: ");
Serial.print(Output);
Serial.println();
}
Drive code:
void drive()
{
if (Front > 160) //160mm
{
Lmotor = motorNeutral - Output;
Rmotor = motorNeutral + Output;
}
if (Front < 160)
{
Lmotor = 0;
Rmotor = speedmax;
}
// limit rightMotorSpeed
if (Rmotor > 200)
Rmotor = 200;
else if (Rmotor < 0)
Rmotor = 0;
// limit rightMotorSpeed
if (Lmotor > 200)
Lmotor = 200;
else if (Lmotor < 0)
Lmotor = 0;
motor1.run(FORWARD);
motor2.run(FORWARD);
motor1.setSpeed(Lmotor);
motor2.setSpeed(Rmotor);
}
Sensor information:
void readUltrasonicSensors1()
{
// ultrasonic 2
digitalWrite(ultrasonic2TrigPin1, HIGH);
delayMicroseconds(10); // must keep the trig pin high for at least 20us
digitalWrite(ultrasonic2TrigPin1, LOW);
ultrasonic2Duration1 = pulseIn(utlrasonic2EchoPin1, HIGH);
Front = (ultrasonic2Duration1 / 2 ) / 3;
}
void readUltrasonicSensors2()
{
// ultrasonic 2
digitalWrite(ultrasonic2TrigPin2, HIGH);
delayMicroseconds(10); // must keep the trig pin high for at least 10us
digitalWrite(ultrasonic2TrigPin2, LOW);
ultrasonic2Duration2 = pulseIn(utlrasonic2EchoPin2, HIGH);
Input = (ultrasonic2Duration2 / 2 ) / 3;
}
Thank you for any help.