RoboterCC - Robotic Code Compiler
Forum Robot Kits NIBO burger NIBO burger Projekte Linienfolge Grau

Welcome

Nachrichten

Sie sind nicht eingeloggt.

Werbung

Letzte Themen

  • Keine Beiträge vorhanden

Site-Statistic

  • 6298 private projects
  • 359 public projects
  • 10659283 lines compiled
  • 46628 builds
NIBO @ facebook YouTube Twitter
Willkommen, Gast
Benutzername: Passwort: Angemeldet bleiben:
  • Seite:
  • 1

THEMA: Linienfolge Grau

Linienfolge Grau 3 Jahre 11 Monate her #3455

  • Georg
  • Georgs Avatar
  • OFFLINE
  • Gold Boarder
  • Beiträge: 283
Hallo,

18 mm breites schwarzes Isolierband als Linie ist auf der hellen Rückseite einer Lederimitation geklebt. Die Lederimitation gibt es als Meterware von der Rolle in einem Möbelmarkt.
Für die Linienverfolgung wird das Signal der farbigen Bodensensoren verwendet.
Im Programm werden zwei PID-Regler (Arduino PID-Library) verwendet.

Mit dem Taster 1 wird zu Beginn eine Messung im hellen Bereich neben der Linie durchgeführt.
Danach wird der Roboter auf die Linie gestellt.
Mit dem Taster 2 wird die Messung auf der Stelle gestartet.
Der Roboter wird von Hand über der Linie hin und her bewegt, bis beide blauen LEDs leuchten.
Mit dem Taster 3 werden danach die Motoren gestartet.
#include <PID_v1.h>
/*
 * 
***************************************************************
* Arduino PID Library - Version 1.1.1
* by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
*
* This Library is licensed under a GPLv3 License
***************************************************************
 
 - For an ultra-detailed explanation of why the code is the way it is, please visit: 
   http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/
 
 - For function documentation see:  http://playground.arduino.cc/Code/PIDLibrary
 */
 
#include <NIBOburger.h>

float l_max, c_max, r_max;      // Messungen im Feld (100%)
float l, c, r;                  // Messwerte Links, Center, Rechts
double l_minus_r;               // Differenz Messwert Links - Rechts
int16_t pwm_speed = 800;        // Maximale Geschwindigkeit
int16_t pwm_links = pwm_speed;  // Geschwindigkeit linker Motor
int16_t pwm_rechts = pwm_speed; // Geschwindigkeit rechter Motor
char key = ' ';
double Output_links, Output_rechts, Setpoint = 0.0; // Regler-Output im Bereich von 0 bis. 255
double Kp = 2.0 , Kd = 0.5, Ki = 0;
 
// PID_rechts wertet l_minus_r im Bereich von -100 bis 0 aus
// Im Bereich von 0 bis 100 ist der Output_rechts == 0
PID PID_rechts(&l_minus_r, &Output_rechts, &Setpoint, Kp, Ki, Kd, REVERSE);
 
// PID_links wertet l_minus_r im Bereich von 0 .bis 100 aus
// Im Bereich von -100 bis 0 ist der Output_links == 0
PID PID_links(&l_minus_r, &Output_links, &Setpoint, Kp, Ki, Kd, DIRECT);
 
enum
{
  mitte,
  mitte_links,
  mitte_rechts,
  links,
  rechts,
  links_ausserhalb,
  rechts_ausserhalb,
  absturz
}  lage; // Auswertung des Messen über/entlang der Linie
uint8_t lage_old=13;
 
// Untergrund im Feld abseits der Linie messen
void Untergrund()
{
  lage = absturz;
  NIBOburger.setLeds(1,1,1,1);
 
  // Nach dem Drücken der Taste 1 wird der Untegrund gemessen
  while (key != 'A')
  {
    key = NIBOburger.getKeyChar();
  }
  NIBOburger.setLeds(0,0,0,0);
 
  //Serial.println("Messung im Feld");
  //NIBOburger.waitAnalogUpdate();
  c_max = 100. / SurfaceSensor.getC(0);
  //NIBOburger.waitAnalogUpdate();
  //Serial.print("C = ");
  //Serial.println(c_max,DEC);
 
  l_max = 100. /SurfaceSensor.getL(0);
  //NIBOburger.waitAnalogUpdate();
  //Serial.print("L = ");
  //Serial.println(l_max,DEC);
 
  r_max = 100. /SurfaceSensor.getR(0);
  //NIBOburger.waitAnalogUpdate();
  //Serial.print("R = ");
  //Serial.println(r_max,DEC);
 
  //  Nach dem Drücken der Taste 2 wird die Messung gestartet.
  // Der Roboter muss so über der Linie positioniert werden.
  // Über der Linienmitte leuten die beiden blauen LEDs.
  // Danach werden die Motoeren mit Taste 3 gestartet
  while (key != 'B')
  {
    key = NIBOburger.getKeyChar();
  }
  key = ' ';
  while (key != 'C')
  {
    Messen();
    leds();
    key = NIBOburger.getKeyChar();
  }
}
 
 
// Messen über/entlang der Linie
void Messen()
{
  // Die Messwerte der 3 Sensoren werden direkt in %-Werte umgerechnt.
  float c = c_max * SurfaceSensor.getC(0);
  //NIBOburger.waitAnalogUpdate();
  //Serial.print("C%;");
  //Serial.print(c,DEC);
 
 
  float l = l_max * SurfaceSensor.getL(0);
  //NIBOburger.waitAnalogUpdate();
  //Serial.print(";L%;");
  //Serial.print(l,DEC);
 
 
  float r = r_max * SurfaceSensor.getR(0);
  //NIBOburger.waitAnalogUpdate();
  //Serial.print(";R%;");
  //Serial.print(r,DEC);
 
  // l_minus_r ist der Input für die beiden PID-Regler
  // Der Sollwert ist 0.0
  l_minus_r = l - r;
  //Serial.print(";l-r;");
  //Serial.println(l_minus_r,DEC);
 
 
  //***********************************
  // Auswertung der Messwerte
  // Im  Regelbereich
  if (c < 40.)
  {
    // Roboter ist über der Linienmitte im Regelbereich
    if (abs(l_minus_r) < 20.)
    {
      //NIBOburger.setLeds(0,1,1,0);
      lage = mitte;
    }
    // Roboter ist rechts von der Linienmitte im Regelbereich
    else if (abs(l_minus_r) == (l_minus_r))
    {
      //NIBOburger.setLeds(0,0,1,0);
      lage = mitte_rechts;
    }
    // Roboter ist links von der Linienmitte im Regelbereich
    else
    {
      //NIBOburger.setLeds(0,1,0,0);
      lage = mitte_links;
    }
  }
  else // (c >=40)
  {
    // Rechts außerhalb des Regelbereiches unter dem rechten Sensor
    if (r < 80.)
    {
      //NIBOburger.setLeds(0,0,1,1);
      lage = rechts;
    }
    // Links außerhalb des Regelbereiches unter dem linken Sensor
    else if (l < 80.)
    {
      //NIBOburger.setLeds(1,1,0,0);
      lage = links;
    }
    // Im Feld, Roboter hat die Linie verloren
    else if (lage == rechts)
    {
      //NIBOburger.setLeds(0,0,0,1);
      lage = rechts_ausserhalb;
    }
    else if (lage == links)
    {
      //NIBOburger.setLeds(1,0,0,0);
      lage = links_ausserhalb;
    }
  }
}
 
void Fahren()
{
  switch (lage) {
    // Bei lage == ..._ausserhalb Drehung auf der Stelle zur Linie hin
    case links_ausserhalb:
    pwm_links = -pwm_speed/2;
    pwm_rechts = pwm_speed/2;
    break;
 
    // Bei lage == ..._ausserhalb Drehung auf der Stelle zur Linie hin
    case rechts_ausserhalb:
    pwm_rechts = -pwm_speed/2;
    pwm_links = pwm_speed/2;
    break;
 
    // Im PID-Regelbereich
    default:
    // Berechnung
    while (!PID_rechts.Compute());
    while (!PID_links.Compute());
    //Output_links und Output_rechts sind im Bereich von 0 bis 255
    pwm_links = pwm_speed - (Output_links * pwm_speed/255.);
    pwm_rechts = pwm_speed - (Output_rechts * pwm_speed/255.);
    pwm_links = constrain(pwm_links, 0, 1024);
    pwm_rechts = constrain(pwm_rechts, 0, 1024);
    break;
 
  }
  Engine.setPWM(pwm_links,pwm_rechts);
}
 
// LED-Anzeige
void leds()
{
  if (lage_old == lage) return;
  switch (lage)
  {
    case mitte:
    NIBOburger.setLeds(0,1,1,0);
    break;
    case mitte_rechts:
    NIBOburger.setLeds(0,0,1,0);
    break;
    case mitte_links:
    NIBOburger.setLeds(0,1,0,0);
    break;
    case rechts:
    NIBOburger.setLeds(0,0,1,1);
    break;
    case links:
    NIBOburger.setLeds(1,1,0,0);
    break;
    case rechts_ausserhalb:
    NIBOburger.setLeds(0,0,0,1);
    break;
    case links_ausserhalb:
    NIBOburger.setLeds(1,0,0,0);
 
  }
  lage_old == lage;
}
// Setup
void setup() {
  NIBOburger.begin();
  //Serial.begin(9600);
  Engine.begin();
  SurfaceSensor.begin();
  PID_rechts.SetMode(AUTOMATIC);
  PID_rechts.SetSampleTime(30);
 
  PID_links.SetMode(AUTOMATIC);
  PID_links.SetSampleTime(30);
 
  Untergrund();
  Engine.setPWM(pwm_links,pwm_rechts);
}
 
void loop() {
  Messen();
  Fahren();
  leds();
}

lg Georg
Letzte Änderung: 3 Jahre 11 Monate her von Georg.
Der Administrator hat öffentliche Schreibrechte deaktiviert.
  • Seite:
  • 1
Ladezeit der Seite: 0.181 Sekunden

Werbung

Online

Keiner