This is an old revision of the document!

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) {

// TODO: bessere Umrechnung (z.B. [0,1]->[500,799] und [-1,0]->[-799,-500])
int l = links*799;
int r = 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 TODO: bessere Werte finden float pid[] = {1.0, 0.0, 0.0};

void lineFollow() {

// Diese Werte zuerst experimentell bestimmen!
int weiss = 600;  // Messwert auf dem weissen Klebeband
int schwarz = 80;  // 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.5;

// 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 = 0.0; // TODO: hier Formel aus wert, weiss und schwarz
  float zeit = (micros()-lastus)/10000000.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)?
    break; // Wiederholung abbrechen
  }
  first = false;  // Ist nicht mehr das erste Mal
}
robot.motors.setPowers(0,0); // Motoren Power 0 (drehen sich aber noch ca. 5 Ticks weiter!)
robot.buttons.waitClear();  // Warten bis Dfingerabdäröschti

}

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);
}

}