Differences
This shows you the differences between two versions of the page.
Both sides previous revision Previous revision Next revision | Previous revision | ||
lehrkraefte:blc:robotics:pid [2017/07/04 07:55] Ivo Blöchliger |
lehrkraefte:blc:robotics:pid [2017/07/07 07:49] (current) Ivo Blöchliger |
||
---|---|---|---|
Line 1: | Line 1: | ||
- | + | ===== Funktionierenden Line-Follower ===== | |
- | + | <code c++ sketch.txt> | |
- | <code c++> | + | |
// Umrechnung vom Intervall [-1,1] auf [-799,799] | // Umrechnung vom Intervall [-1,1] auf [-799,799] | ||
// Die Umrechnung braucht nicht linear zu sein! Die Motoren drehen schliesslich erst ab ca. Power 500 | // Die Umrechnung braucht nicht linear zu sein! Die Motoren drehen schliesslich erst ab ca. Power 500 | ||
void setFloatPower(float links, float rechts) { | void setFloatPower(float links, float rechts) { | ||
- | | + | |
- | int l = links*799; | + | int r = ((rechts>=0) ? 1 : -1)*(sqrt(abs(rechts)))*799; |
- | int r = rechts*799; | + | |
robot.motors.setPowers(l, | robot.motors.setPowers(l, | ||
} | } | ||
Line 23: | Line 21: | ||
// Parameter P,I,D | // Parameter P,I,D | ||
- | // TODO: bessere Werte finden | + | float pid[] = {0.9, 0.0, 0.03}; |
- | float pid[] = {1.0, 0.0, 1.0}; | + | |
- | void lineFollow() { | + | |
+ | /** | ||
+ | | ||
+ | */ | ||
+ | int lineFollow(int dir) { | ||
// Diese Werte zuerst experimentell bestimmen! | // Diese Werte zuerst experimentell bestimmen! | ||
- | int weiss = 600; // Messwert auf dem weissen Klebeband | + | int weiss = 380; // Messwert auf dem weissen Klebeband |
- | int schwarz = 80; // Messwert auf dem schwarzen Klebeband | + | int schwarz = 30; // Messwert auf dem schwarzen Klebeband |
| | ||
// Sollwert für die Motoren (in [-1,1]), hängt auch vom Unterprogram setFloatPower ab! | // 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. | // D.h. wie schnell sollen diese drehen, wenn man genau auf der Linie fährt. | ||
- | float soll = 0.5; | + | float soll = 0.65; |
// Zeitpunkt letzter Messung | // Zeitpunkt letzter Messung | ||
Line 52: | Line 53: | ||
int wert = ir.measure(); | int wert = ir.measure(); | ||
// Auf Intervall [-1,1] umrechnen, v ist damit auch die Abweichung vom Sollwert 0.0 | // 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 v = dir*(2.0*(wert-schwarz)/(weiss-schwarz) - 1.0); |
- | float zeit = (micros()-lastus)/ | + | float zeit = (micros()-lastus)/ |
+ | lastus = micros(); // Zeit merken | ||
float diff = (lastv-v)/ | float diff = (lastv-v)/ | ||
lastv = v; // Messwert merken | lastv = v; // Messwert merken | ||
Line 60: | Line 62: | ||
diff = 0.0; // Differenz macht beim ersten Mal keinen Sinn | diff = 0.0; // Differenz macht beim ersten Mal keinen Sinn | ||
} | } | ||
- | float korrektur = pid[0]*v + pid[1]*summe | + | float korrektur = pid[0]*v + pid[1]*summe |
float links=clip(soll+korrektur); | float links=clip(soll+korrektur); | ||
float rechts=clip(soll-korrektur); | float rechts=clip(soll-korrektur); | ||
setFloatPower(links, | setFloatPower(links, | ||
if (robot.buttons.get()!=0) { // Button gedrückt (nicht Null)? | if (robot.buttons.get()!=0) { // Button gedrückt (nicht Null)? | ||
- | break; | + | |
+ | | ||
} | } | ||
+ | | ||
+ | // | ||
+ | // TODO: Weitere Abbruch-Bedingungen formulieren | ||
+ | // | ||
+ | | ||
first = false; | first = false; | ||
} | } | ||
- | robot.buttons.waitClear(); | ||
} | } | ||
Line 78: | Line 85: | ||
char *msg[] = {"Set proportional", | char *msg[] = {"Set proportional", | ||
while (true) { | while (true) { | ||
- | int choice = robot.menu.choice(" | + | int choice = robot.menu.choice(" |
if (choice==4) { | if (choice==4) { | ||
break; | break; | ||
} | } | ||
- | // Anpassen mit Menufunktion | + | // 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] = robot.menu.adjustFloat(msg[choice-1], | ||
+ | } | ||
} | } | ||
+ | |||
</ | </ |