-         

Ergebnis 1 bis 7 von 7

Thema: Suche guten Quelltext für PID Linienfolger (Panzerchassis)

  1. #1

    Suche guten Quelltext für PID Linienfolger (Panzerchassis)

    Anzeige

    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

  2. #2
    Erfahrener Benutzer Robotik Einstein
    Registriert seit
    09.10.2014
    Beiträge
    2.435
    du weißt aber schon, dass es eine PID lib für Arduino gibt, oder?
    ·±≠≡≈³αγελΔΣΩ∞ Schachroboter:www.youtube.com/watch?v=Cv-yzuebC7E Rasenmäher-Robot:www.youtube.com/watch?v=z7mqnaU_9A8

  3. #3
    Zitat Zitat von HaWe Beitrag anzeigen
    du weißt aber schon, dass es eine PID lib für Arduino gibt, oder?
    Jetzt ja

    Ich habe die jedoch noch bei keinem der bisher gefundenen Linienfolger Projekte implementiert gesehen.
    Alle Quelltexte haben sich irgendwie ein bisschen PID Regler darin selber zusammengeschustert.

  4. #4

  5. #5
    RN-Premium User Roboter-Spezialist Avatar von witkatz
    Registriert seit
    24.05.2006
    Ort
    NRW
    Alter
    47
    Beiträge
    458
    Blog-Einträge
    16
    Zitat Zitat von julianpe Beitrag anzeigen
    Hat jemand einen Tipp,
    1. Zuerst solltest du die Sensorik inbetriebnehmen. Also den Positionswert auf der Debugausgabe beobachten, ob der Sensor im ganzen Erfassungsbereich auf die Linie vernünftig reagiert.
    2. Den Richtungssinn feststellen, also wächst oder sinkt der Positionswert, wenn die Linie links oder rechts der Mitte ist? Eventuell muss die Reglerausgabe umgekehrt auf Links/Rechts drauf.
    3. Kp immer weiter erhöhen, bis der Roboter auf die Linie reagiert. Dann mit Kp und Kd experimentieren...

    Gruß
    witkatz
    Geändert von witkatz (14.11.2015 um 21:27 Uhr)

  6. #6
    Meine Debug Ausgabe sieht wie folgt aus:

    Klicke auf die Grafik für eine größere Ansicht

Name:	Sensoren.jpg
Hits:	11
Größe:	33,8 KB
ID:	30884

    Die Szenarien sind wie folgt definiert.

    Klicke auf die Grafik für eine größere Ansicht

Name:	SensorSzenarien.png
Hits:	9
Größe:	9,0 KB
ID:	30885

    Müsste doch soweit alles in Ordnung sein?
    Geändert von julianpe (15.11.2015 um 09:52 Uhr)

  7. #7
    RN-Premium User Roboter-Spezialist Avatar von witkatz
    Registriert seit
    24.05.2006
    Ort
    NRW
    Alter
    47
    Beiträge
    458
    Blog-Einträge
    16
    Bei PWM Ausgang 255 wird der Motor schneller? Wenn ich das richtig interpretiere, dann ist der Richtungssinn deines Regler falsch rum. Die Linie ist links der Mitte -> Fehlerwert ist negativ -> der Linke Motor wird beschleunigt und der rechte gebremst -> Roboter fährt nach rechts und entfernt sich noch weiter von der Linie. Oder?
    Versuch es vielleicht mal mit
    Code:
    int rightMotorSpeed = MOTOR_SPEED - motorSpeed; 
    int leftMotorSpeed  = MOTOR_SPEED + motorSpeed;
    Du kannst auch am Anfang Kd auf 0 setzen und nur mit Kp den Regler in Gang setzen. Erst wenn ein einfacher P-Regler halbwegs funktioniert, solltest du an der Reglerotpimierung weitermachen - deinen PD Regler oder verschiedene PIDs aus diversen Librarys testen und vergleichen.

    Gruß
    witkatz

Ähnliche Themen

  1. Linienfolger mit Panzerchassis?
    Von julianpe im Forum Allgemeines zum Thema Roboter / Modellbau
    Antworten: 1
    Letzter Beitrag: 02.11.2015, 23:10
  2. Suche guten Screen Recorder
    Von Herkules9987 im Forum Offtopic und Community Tratsch
    Antworten: 2
    Letzter Beitrag: 04.06.2013, 09:59
  3. suche guten Platinenhalter.
    Von Viecherl im Forum Suche bestimmtes Bauteil bzw. Empfehlung
    Antworten: 17
    Letzter Beitrag: 07.07.2011, 12:51
  4. suche guten bausatz
    Von mario5555 im Forum Robby CCRP5
    Antworten: 1
    Letzter Beitrag: 13.02.2009, 17:48
  5. suche guten C-Compiler
    Von Zeroeightfifteen im Forum C - Programmierung (GCC u.a.)
    Antworten: 14
    Letzter Beitrag: 08.11.2006, 19:15

Berechtigungen

  • Neue Themen erstellen: Nein
  • Themen beantworten: Nein
  • Anhänge hochladen: Nein
  • Beiträge bearbeiten: Nein
  •