Hello !
I have some problems to add a PID regulation on my line follower robot. First i use a QTR 8A sensor and 2 servomotors and a pusher buton to control the robot. I tryed to do as it's explain in this website :
For my PID regulation i choosed to use the PID_v1 library, a setpoint of 2500 (represents the lines under the middle of the sensors)
and i choosed the Output beetwen -89 and 89 !
But i have 2 principal problem, first my servo motors are on opposed direction that mean, if i write :
myservoLeft.write(180); the left motor has the maximal speed to make the robot move forward
myservoRight.write(0); the Right motor has the maximal speed to make the robot move forward too.
But with this previous lines, i can't use these equations given in the website :
RightMotorSpeed = RightBaseSpeed + MotorSpeed;
LeftMotorSpeed = LeftBaseSpeed - MotorSpeed;
So, how i can change these equation to create a good correction ?
Moreover my Ouput is very instable, indeed it can jump from -89 to 89 without any reason, so i think this is a Constants problem, so what i have to do to decrease the instability ?
The last probleme is that if i choose for exemple an Output between 0 and 180, if the position is superior than 2500 the Output is always 0, but i don't know why ?
#include <QTRSensors.h>
#include <Servo.h>
#include <PID_v1.h>
double Setpoint ; // 2500
double Input; // QTR 8A
double Output ; //SERVO
//PID parameters
double Kp=0.3, Ki=0.3, Kd=0.1;
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
// This example is designed for use with six analog QTR sensors. These
// reflectance sensors should be connected to analog pins A0 to A5. The
// sensors' emitter control pin (CTRL or LEDON) can optionally be connected to
// digital pin 2, or you can leave it disconnected and remove the call to
// setEmitterPin().
//
// The setup phase of this example calibrates the sensors for ten seconds and
// turns on the Arduino's LED (usually on pin 13) while calibration is going
// on. During this phase, you should expose each reflectance sensor to the
// lightest and darkest readings they will encounter. For example, if you are
// making a line follower, you should slide the sensors across the line during
// the calibration phase so that each sensor can get a reading of how dark the
// line is and how light the ground is. Improper calibration will result in
// poor readings.
//
// The main loop of the example reads the calibrated sensor values and uses
// them to estimate the position of a line. You can test this by taping a piece
// of 3/4" black electrical tape to a piece of white paper and sliding the
// sensor across it. It prints the sensor values to the serial monitor as
// numbers from 0 (maximum reflectance) to 1000 (minimum reflectance) followed
// by the estimated location of the line as a number from 0 to 5000. 1000 means
// the line is directly under sensor 1, 2000 means directly under sensor 2,
// etc. 0 means the line is directly under sensor 0 or was last seen by sensor
// 0 before being lost. 5000 means the line is directly under sensor 5 or was
// last seen by sensor 5 before being lost.
QTRSensors qtr;
const uint8_t SensorCount = 6;
uint16_t sensorValues[SensorCount];
Servo myservoLeft;
Servo myservoRight;
int compteurButton = 0;
int tour = 1;
int myservoLeftspeed = 180;
int myservoRightspeed = 0;
void setup()
{
myservoLeft.attach(5);
myservoRight.attach(3);
pinMode(10, INPUT_PULLUP);
Setpoint = 2500;
//Turn the PID on
myPID.SetMode(AUTOMATIC);
//Adjust PID values
myPID.SetTunings(Kp, Ki, Kd);
myPID.SetSampleTime(1);
myPID.SetOutputLimits(-89, 89);
}
void loop()
{
int test = digitalRead(10);
if(test == LOW){
compteurButton = compteurButton + 1;
delay(1000);
}
if(tour == 1){
myservoLeft.write(90);
myservoRight.write(90);
}
if(compteurButton == 1 && tour == 1){
tour = tour + 1;
// configure the sensors
qtr.setTypeAnalog();
qtr.setSensorPins((const uint8_t[]){A0, A1, A2, A3, A4, A5}, SensorCount);
qtr.setEmitterPin(2);
delay(500);
pinMode(LED_BUILTIN, OUTPUT);
digitalWrite(LED_BUILTIN, HIGH); // turn on Arduino's LED to indicate we are in calibration mode
myservoLeft.write(92);
myservoRight.write(92);
// analogRead() takes about 0.1 ms on an AVR.
// 0.1 ms per sensor * 4 samples per sensor read (default) * 6 sensors
// * 10 reads per calibrate() call = ~24 ms per calibrate() call.
// Call calibrate() 400 times to make calibration take about 10 seconds.
for (uint16_t i = 0; i < 100; i++)
{
qtr.calibrate();
if(i>25){
myservoLeft.write(88);
myservoRight.write(88);
}
if(i>75){
myservoLeft.write(90);
myservoRight.write(90);
}
}
digitalWrite(LED_BUILTIN, LOW); // turn off Arduino's LED to indicate we are through with calibration
// print the calibration minimum values measured when emitters were on
Serial.begin(9600);
for (uint8_t i = 0; i < SensorCount; i++)
{
Serial.print(qtr.calibrationOn.minimum[i]);
Serial.print(' ');
}
Serial.println();
// print the calibration maximum values measured when emitters were on
for (uint8_t i = 0; i < SensorCount; i++)
{
Serial.print(qtr.calibrationOn.maximum[i]);
Serial.print(' ');
}
Serial.println();
Serial.println();
delay(1000);
}
// read calibrated sensor values and obtain a measure of the line position
// from 0 to 5000 (for a white line, use readLineWhite() instead)
// print the sensor values as numbers from 0 to 1000, where 0 means maximum
// reflectance and 1000 means minimum reflectance, followed by the line
// position
uint16_t position = qtr.readLineBlack(sensorValues);
if(compteurButton == 2){
Input = position;
myPID.Compute();
double errorMotor = Output;
if(Input>2500){
int myservoLeftspeed = 180;
int myservoRightspeed = 90;
Serial.println("Input > 2500 ");
}
else if(Input<2500){
int myservoLeftspeed = 90;
int myservoRightspeed = 0;
Serial.println("Input < 2500 ");
}
int servoLeftSpeed = myservoLeftspeed + errorMotor;
int servoRightSpeed = myservoRightspeed + errorMotor;
if(servoLeftSpeed < 90)servoLeftSpeed = 90;
if(servoRightSpeed > 90)servoRightSpeed = 90;
if(servoLeftSpeed > 180)servoLeftSpeed = 180;
if(servoRightSpeed < 0)servoRightSpeed = 0;
Serial.print("Input ");
Serial.println(Input);
Serial.print("Servo L ");
Serial.println(servoLeftSpeed);
Serial.print("Servo R ");
Serial.println(servoRightSpeed);
Serial.print("Error ");
Serial.println(errorMotor);
myservoLeft.write(servoLeftSpeed);
myservoRight.write(servoRightSpeed);
}
}
Thanks for your help !
PS : Sorry for my bad English..