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