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) { return f>1.0 ? 1.0 : (f<-1.0 ? -1.0 : f); } // Parameter P,I,D // TODO: bessere Werte finden float pid[] = {1.0, 0.0, 1.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 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.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("setP", "setI", "setD", "abort"); if (choice==4) { break; } // Anpassen mit Menufunktion pid[choice-1] = robot.menu.adjustFloat(msg[choice-1], pid[choice-1], abs(pid[choice-1])/20.0+0.001); }