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
Zitieren
Zitat von HaWe
Lesezeichen