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/06 09:23] Simon Knaus |
lehrkraefte:blc:robotics:pid [2017/07/07 07:49] (current) Ivo Blöchliger |
||
---|---|---|---|
Line 1: | Line 1: | ||
+ | ===== Funktionierenden Line-Follower ===== | ||
+ | <code c++ sketch.txt> | ||
// 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 20: | 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, 0.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 49: | 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 | lastus = micros(); // Zeit merken | ||
float diff = (lastv-v)/ | float diff = (lastv-v)/ | ||
Line 58: | 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.motors.setPowers(0, | ||
- | robot.buttons.waitClear(); | ||
} | } | ||
Line 83: | Line 91: | ||
// Anpassen mit Menufunktion, | // Anpassen mit Menufunktion, | ||
pid[choice-1] = robot.menu.adjustFloat(msg[choice-1], | pid[choice-1] = robot.menu.adjustFloat(msg[choice-1], | ||
- | } | + | |
} | } | ||
+ | |||
+ | |||
+ | </ |