Suche guten Quelltext für PID Linienfolger (Panzerchassis)
Hallo,
ich versuche momentan einen Linienfolger mit Hilfe der Pololu QTR-8RC Sensoren und dem Pololu RP5 Chassis zu realisieren.
Nutze dazu einen Arduino Mega sowie das Arduino Motor Shield.
Nun möchte ich die Ansteuerung, der Motoren, bzw die Regelung mittels PID Regler implementieren.
Bin dazu auf folgende Anleitung gestoßen: http://letsmakerobots.com/blog/enigm...line-following
Originalcode:
Code:
#include <QTRSensors.h>
#define Kp 0 // experiment to determine this, start by something small that just makes your bot follow the line at a slow speed
#define Kd 0 // experiment to determine this, slowly increase the speeds and adjust this value. ( Note: Kp < Kd)
#define rightMaxSpeed 200 // max speed of the robot
#define leftMaxSpeed 200 // max speed of the robot
#define rightBaseSpeed 150 // this is the speed at which the motors should spin when the robot is perfectly on the line
#define leftBaseSpeed 150 // this is the speed at which the motors should spin when the robot is perfectly on the line
#define NUM_SENSORS 6 // number of sensors used
#define TIMEOUT 2500 // waits for 2500 us for sensor outputs to go low
#define EMITTER_PIN 2 // emitter is controlled by digital pin 2
#define rightMotor1 3
#define rightMotor2 4
#define rightMotorPWM 5
#define leftMotor1 12
#define leftMotor2 13
#define leftMotorPWM 11
#define motorPower 8
QTRSensorsRC qtrrc((unsigned char[]) { 14, 15, 16, 17, 18, 19} ,NUM_SENSORS, TIMEOUT, EMITTER_PIN); // sensor connected through analog pins A0 - A5 i.e. digital pins 14-19
unsigned int sensorValues[NUM_SENSORS];
void setup()
{
pinMode(rightMotor1, OUTPUT);
pinMode(rightMotor2, OUTPUT);
pinMode(rightMotorPWM, OUTPUT);
pinMode(leftMotor1, OUTPUT);
pinMode(leftMotor2, OUTPUT);
pinMode(leftMotorPWM, OUTPUT);
pinMode(motorPower, OUTPUT);
int i;
for (int i = 0; i < 100; i++) // calibrate for sometime by sliding the sensors across the line, or you may use auto-calibration instead
/* comment this part out for automatic calibration
if ( i < 25 || i >= 75 ) // turn to the left and right to expose the sensors to the brightest and darkest readings that may be encountered
turn_right();
else
turn_left(); */
qtrrc.calibrate();
delay(20);
wait();
delay(2000); // wait for 2s to position the bot before entering the main loop
/* comment out for serial printing
Serial.begin(9600);
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMinimumOn[i]);
Serial.print(' ');
}
Serial.println();
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMaximumOn[i]);
Serial.print(' ');
}
Serial.println();
Serial.println();
*/
}
int lastError = 0;
void loop()
{
unsigned int sensors[6];
int position = qtrrc.readLine(sensors); // get calibrated readings along with the line position, refer to the QTR Sensors Arduino Library for more details on line position.
int error = position - 2500;
int motorSpeed = Kp * error + Kd * (error - lastError);
lastError = error;
int rightMotorSpeed = rightBaseSpeed + motorSpeed;
int leftMotorSpeed = leftBaseSpeed - motorSpeed;
if (rightMotorSpeed > rightMaxSpeed ) rightMotorSpeed = rightMaxSpeed; // prevent the motor from going beyond max speed
if (leftMotorSpeed > leftMaxSpeed ) leftMotorSpeed = leftMaxSpeed; // prevent the motor from going beyond max speed
if (rightMotorSpeed < 0) rightMotorSpeed = 0; // keep the motor speed positive
if (leftMotorSpeed < 0) leftMotorSpeed = 0; // keep the motor speed positive
{
digitalWrite(motorPower, HIGH); // move forward with appropriate speeds
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);
analogWrite(rightMotorPWM, rightMotorSpeed);
digitalWrite(motorPower, HIGH);
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
analogWrite(leftMotorPWM, leftMotorSpeed);
}
}
void wait(){
digitalWrite(motorPower, LOW);
}
Habe den entsprechend auf meine Bedürfnisse angepasst.
Ich konnte über die Debug Serial Ausgabe sehen, dass die Kalibrierung und die Erfassung der Sensorwerte soweit
tadellos funktioniert. Jedoch verstehe ich nicht, weshalb der Roboter dann immer von der Strecke abweicht und häufig
nur Kreise dreht, obwohl zwischendurch immer wieder schwarze Linien auftauchen, die er erkennen sollte.
Code:
#include <QTRSensors.h>
#include <Servo.h>
///////////////////////////
// define constants //
///////////////////////////
#define AUTO_CALIB false // Autokalibrierung aktivieren
#define DEBUG_ENABLE true // false=keine Serial Ausgaben
#define NUM_SENSORS 8 // Anzahl der verwendeten Sensoren
#define TIMEOUT 2500 // waits for 2500 microseconds for sensor outputs to go low
#define EMITTER_PIN QTR_NO_EMITTER_PIN // Default: kein Emitterpin definiert
#define MOTOR_SPEED 100 // PWM Signal 0-255 (Basismotorgeschwindigkeit)
#define MOTOR_MAX_SPEED 255 // PWM Signal für Maximalgeschwindigkeit
#define KP 0.2 // KP-Wert für PID Regler
#define KD 5 // KD-Wert für PID Regler ( Beachte: Kp < Kd)
#define SERVO_PIN1 11 // PWM Ausgang für Servo1
#define SERVO_PIN2 12 // PWM Ausgang für Servo2
#define CALIB_LED 13 // LED Ausgang für Kalibrierungsphase
//////////////////////////
// define variables //
//////////////////////////
// Zuweisung der Sensoren 0-7 an die digitalen IO's 22-29
QTRSensorsRC qtrrc((unsigned char[]) {22, 23, 24, 25, 26, 27, 28, 29}, NUM_SENSORS, TIMEOUT, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS];
int MotorLeftSpeed=3; // PWM pin for speed control left
int MotorRightSpeed=11; // PWM pin for speed control right
int MotorLeftDirection=12; // digital pin for direction control left
int MotorRightDirection=13; // digital pin for direction control right
int MotorLeftBrake=9; // digital pin for brake control left
int MotorRightBrake=8; // digital pin for brake control right
int lastError = 0;
/////////////////////
// Initialisierung //
/////////////////////
void setup()
{
// Initialisierung verschiedner IOs
pinMode(CALIB_LED, OUTPUT); // Definiert LED Ausgang für Kalibierungsphase
// Initialisierung linker Motor
pinMode(MotorLeftDirection, OUTPUT); // Definiert Ausgang für den linken Motor
pinMode(MotorLeftBrake, OUTPUT); // Definiert Bremse für den linken Motor
pinMode(MotorLeftSpeed, OUTPUT); // Definiert Geschwindigkeitsausgang für linken Motor
// Initialisierung rechter Motor
pinMode(MotorRightDirection, OUTPUT); // Definiert Ausgang für den rechten Motor
pinMode(MotorRightBrake, OUTPUT); // Definiert Bremse für den rechten Motor
// Initialisierung Servo Motoren
servo1.attach(SERVO_PIN1); // Zuweisung PWM PIN für Servo1
servo2.attach(SERVO_PIN2); // Zuweisung PWM PIN für Servo2
if (DEBUG_ENABLE)
{
Serial.begin(9600);
Serial.print("Debug Modus aktiviert:\n");
}
delay(500);
digitalWrite(CALIB_LED, HIGH); // turn on Arduino's LED to indicate we are in calibration mode
for (int i = 0; i < 400; i++) // make the calibration take about 10 seconds
{
qtrrc.calibrate(); // reads all sensors 10 times at 2500 us per read (i.e. ~25 ms per call)
}
delay(20); // The 20 ms delay in the calibration code is intended to make the routine take a reasonable amount of time.
digitalWrite(CALIB_LED, LOW); // turn off Arduino's LED to indicate we are through with calibration
if (DEBUG_ENABLE)
{
// print the calibration minimum values measured when emitters were on
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMinimumOn[i]);
Serial.print(' ');
}
Serial.println();
// print the calibration maximum values measured when emitters were on
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMaximumOn[i]);
Serial.print(' ');
}
Serial.println();
Serial.println();
}
delay(1000);
}
///////////////////
// Main Program //
///////////////////
void loop()
{
// read calibrated sensor values and obtain a measure of the line position from 0 to 5000
// To get raw sensor values, call:
// qtrrc.read(sensorValues); instead of unsigned int position = qtrrc.readLine(sensorValues);
// Variable positon zeigt die genaue Position (0-7000) an
unsigned int position = qtrrc.readLine(sensorValues); // get calibrated readings along with the line position, refer to the QTR Sensors Arduino Library for more details on line position.
int error = position - 3500; // 3500 is the desired position
if (DEBUG_ENABLE)
{
// 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
for (unsigned char i = 0; i < NUM_SENSORS; i++)
{
Serial.print(sensorValues[i]);
Serial.print('\t');
}
Serial.print(position); // comment this line out if you are using raw values
}
int motorSpeed = KP * error + KD * (error - lastError);
lastError = error;
int rightMotorSpeed = MOTOR_SPEED + motorSpeed;
int leftMotorSpeed = MOTOR_SPEED - motorSpeed;
if (rightMotorSpeed > MOTOR_MAX_SPEED ) rightMotorSpeed = MOTOR_MAX_SPEED; // Verhindert dass die Maximalgeschwindigkeit überschritten wird
if (leftMotorSpeed > MOTOR_MAX_SPEED ) leftMotorSpeed = MOTOR_MAX_SPEED; // Verhindert dass die Maximalgeschwindigkeit überschritten wird
if (rightMotorSpeed < 0) rightMotorSpeed = 0; // Verhindert einen negativen Geschwindigkeitswert
if (leftMotorSpeed < 0) leftMotorSpeed = 0; // Verhindert einen negativen Geschwindigkeitswert
{
// Ansteuerung der Motoren durch PID Berechnung
digitalWrite(MotorRightBrake, LOW); // Motorbremse rechts deaktivieren
digitalWrite(MotorLeftBrake, LOW); // Motorbremse links deaktivieren
digitalWrite(MotorRightDirection, HIGH); // Motor rechts im Uhrzeigersinn --> fährt vorwärts
digitalWrite(MotorLeftDirection, LOW); // Motor links gegen den Uhrzeigersinn --> fährt vorwärts
analogWrite(MotorRightSpeed, rightMotorSpeed); // Motorgeschwindigkeit rechts in Abhängigkeit des PID Reglers
analogWrite(MotorLeftSpeed, leftMotorSpeed); // Motorgeschwindigkeit links in Abhängigkeit des PID Reglers
}
}
Hat jemand einen Tipp, welchen Code ich stattdessen als Grundlagen nehmen sollte, bzw was an meinem Code falsch sein könnte.
Besten Dank
Liste der Anhänge anzeigen (Anzahl: 2)
Meine Debug Ausgabe sieht wie folgt aus:
Anhang 30884
Die Szenarien sind wie folgt definiert.
Anhang 30885
Müsste doch soweit alles in Ordnung sein?
