lehrkraefte:blc:robotics:pid

Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Both sides previous revision Previous revision
lehrkraefte:blc:robotics:pid [2017/07/07 05:48] – Ivo Blöchligerlehrkraefte:blc:robotics:pid [2017/07/07 05:49] (current) – Ivo Blöchliger
Line 1: Line 1:
 +===== Funktionierenden Line-Follower =====
 +<code c++ sketch.txt>
 +// Umrechnung vom Intervall [-1,1] auf [-799,799]
 +// Die Umrechnung braucht nicht linear zu sein! Die Motoren drehen schliesslich erst ab ca. Power 500
 +void setFloatPower(float links, float rechts) {
 +  int l = ((links>=0) ? 1 : -1)*(sqrt(abs(links)))*799;
 +  int r = ((rechts>=0) ? 1 : -1)*(sqrt(abs(rechts)))*799;
 +  robot.motors.setPowers(l,r);
 +}
  
 +// Funktion, die den Werte zwischen -1.0 und 1.0 beschränkt
 +// z.B. ist clip(4.2)=1.0, clip(-0.42)=-0.42 und clip(-42)=-1.0
 +float clip(float f) {
 +  if (f>1.0) {
 +    return 1.0;
 +  } else if (f<-1.0) {
 +    return -1.0;
 +  } 
 +  return f;
 +}
 +
 +// Parameter P,I,D
 +float pid[] = {0.9, 0.0, 0.03};
 +
 +
 +/** 
 + *  Der Linie folgen (dir=-1 heisst schwarz ist links).
 + */
 +int lineFollow(int dir) {
 +  // Diese Werte zuerst experimentell bestimmen!
 +  int weiss = 380;  // Messwert auf dem weissen Klebeband
 +  int schwarz = 30;  // Messwert auf dem schwarzen Klebeband
 +  
 +  // Sollwert für die Motoren (in [-1,1]), hängt auch vom Unterprogram setFloatPower ab!
 +  // D.h. wie schnell sollen diese drehen, wenn man genau auf der Linie fährt.
 +  float soll = 0.65;
 +
 +  // Zeitpunkt letzter Messung
 +  long lastus = micros(); // Systemzeit in Microsekunden
 +
 +  // Wert letzter Messung
 +  float lastv = 0.0;
 +
 +  // Wahr/falsch Variable für ersten Durchgang
 +  bool first = true;
 +
 +  // Summe der Abweichungen
 +  float summe = 0.0;
 +
 +  // Wiederholen
 +  while (true) {
 +    // Werte messen (liefert Ganzzahl in [0,1023]
 +    int wert = ir.measure();
 +    // Auf Intervall [-1,1] umrechnen, v ist damit auch die Abweichung vom Sollwert 0.0
 +    float v = dir*(2.0*(wert-schwarz)/(weiss-schwarz) - 1.0);
 +    float zeit = (micros()-lastus)/1000000.0;   // Zeitdifferenz in Sekunden
 +    lastus = micros(); // Zeit merken
 +    float diff = (lastv-v)/zeit;  // Änderung pro Zeit
 +    lastv = v;   // Messwert merken
 +    summe = summe + v*zeit;     // Summe der Abweichung, gewichtet mit der Zeit
 +    if (first) {
 +      diff = 0.0;  // Differenz macht beim ersten Mal keinen Sinn
 +    }
 +    float korrektur = pid[0]*v + pid[1]*summe - pid[2]*diff;
 +    float links=clip(soll+korrektur);
 +    float rechts=clip(soll-korrektur);
 +    setFloatPower(links, rechts);
 +    if (robot.buttons.get()!=0) { // Button gedrückt (nicht Null)?
 +      robot.motors.setPowers(0,0);
 +      break;
 +    }
 +    
 +    //
 +    // TODO: Weitere Abbruch-Bedingungen formulieren
 +    //
 +    
 +    first = false;  // Ist nicht mehr das erste Mal
 +  }
 +}
 +
 +// PID-Konstanten über das Menü anpassen.
 +// Achtung, die Konstanten werden bei einem Reset gelöscht. Gute Konstanten im Code oben eintragen!
 +void adjust_pid() {
 +  // Titel beim Anpassen
 +  char *msg[] = {"Set proportional", "Set integral", "Set differential"};
 +  while (true) {
 +    int choice = robot.menu.choice("1 setP", "2 setI", "3 setD", "4 abort");
 +    if (choice==4) {
 +      break;
 +    }
 +    // Anpassen mit Menufunktion, choice ist 1, 2 oder 3. Anpassungsschritt proportional zur Grösse des Parameters
 +    pid[choice-1] = robot.menu.adjustFloat(msg[choice-1], pid[choice-1], abs(pid[choice-1])/20.0+0.001);
 +  }
 +}
 +
 +
 +</code>