Hallo,
ich habe Code ergänzt um die Stanley Paramter an die Sollgeschwindigkeit vom Mäher anzupassen bzw. linear zu mappen.
Das wird besonders in Verbindung mit Adaptiver Mähgeschwindigkeit benötigt.
Bei mir läuft es. Einzufügen in der robot.cpp, andere Änderungen sind nicht notwendig.
direkt darunter einfügen (irgendwo bei Zeile 1220):
else {
if (gps.solution == SOL_FLOAT)
linear = min(setSpeed, FLOATSPEED); // reduce speed for float solution
else
linear = setSpeed; // desired speed
if (sonar.nearObstacle()) linear = SONARSPEED; // slow down near obstacles
}
//angula r = 3.0 * trackerDiffDelta + 3.0 * lateralError; // correct for path errors
////////////////////////////////////X
//Mapping of Stanley Control Parameters in relation to actual Setpoint value of speed
//Values need to be multiplied, because map() function does not work well with decimals
float CurrSpeed = motor.linearSpeedSet * 1000; //X
CurrSpeed = abs(CurrSpeed);
float k = map(CurrSpeed, 100, 600, STANLEY_CONTROL_K_SLOW*1000, STANLEY_CONTROL_K_NORMAL*1000); //X
float p = map(CurrSpeed, 100, 600, STANLEY_CONTROL_P_SLOW*1000, STANLEY_CONTROL_P_NORMAL*1000); //X
k = k / 1000; //X
p = p / 1000; //X
////////////////////////////////////X
Dann direkt darunter auskommentieren bzw. mit obigem ersetzen:
//X float k = STANLEY_CONTROL_K_NORMAL;
//X float p = STANLEY_CONTROL_P_NORMAL;
//X if (maps.trackSlow) {
//X k = STANLEY_CONTROL_K_SLOW;
//X p = STANLEY_CONTROL_P_SLOW;
//X }
Die Werte 100 und 600 sind in mm/s zu verstehen. Hier Könnte man noch MINSPEED und MAXSPEED in der config.h definieren. Habe ich auch gemacht.
Ausserdem kann man dann in der
motor.setlinearangularspeed function noch eine min/max Abfrage des Speeds machen, falls das nicht schon anderweitig passiert.
Grüße,
Sebastian
Edit:
Werte vom Max Speed (von 400 auf 600) geändert.
