Guten Tag!Heute möchte ich die Implementierung des Algorithmus zur Bestimmung der Breite eines Objekts mit einem Ultraschall-Entfernungsmesser und Encodern in der Programmiersprache RobotC (unter Verwendung der VEX EDR-Plattform) teilen.Vorgeschichte
Kürzlich wurde mir angeboten, eine Meisterklasse zur Lösung interessanter Probleme mit der Programmiersprache RobotC durchzuführen. Während der Meisterklasse habe ich mir die Aufgabe überlegt, die Breite des Objekts zu bestimmen, dessen Implementierung ich in dieser Publikation teilen möchte.Elementare Basis
Als experimentelles Übungsgelände verwendete ich die Umgebung von Robot Virtual Worlds für die VEX-Plattform, das virtuelle Robotermodell VEX Launcher, virtuelle Modelle von VRC-Wendepunkt-Wettbewerbselementen (Abbildung 1) und die einfachsten mathematischen Formeln (zur Bestimmung der Länge eines Akkords in einem Kreis mit einem bekannten zentralen Winkel).
Abbildung 1 . Virtueller Übungsplatz mit einem RoboterAufgabe
- Bestimmen des Durchmessers der Kugel nur mit einem Ultraschall-Entfernungsmesser und am Roboter montierten EncodernTheoretische Basis
Um die Breite des Objekts zu bestimmen, habe ich das Konzept eines Strahlungsmusters verwendet (Abbildung 2).
Abbildung 2. Empfindlichkeit und Richtungsmuster des Ultraschall-EntfernungsmessersIm Wesentlichen war es erforderlich, den Winkel zu bestimmen, der beim Drehen von der äußersten rechten Position zur äußersten linken Position zwischen den Tangenten an den äußersten Punkten des Diagramms gebildet wurde.Betrachten Sie Abbildung 3.
Abbildung 3 . AlgorithmusgeometrieWir werden nach dem zentralen Winkel des Bogens suchen, basierend auf der Beziehung zwischen dem Radius des Winkels und dem Drehbogen des Roboters. Der endgültige Breitenwert entspricht dem m - Akkord des Bogens.Eingabeparameter
Als Eingabeparameter habe ich ausgewählt:- dist = 100 mm (der Abstand, in dem der Roboter anhält, um zu messen, der Parameter wird basierend auf dem Vorhandensein anderer Objekte um das Objekt ausgewählt, die in das Sichtfeld des Sensors fallen und die Messung stören können)
- d_wheel = 102 mm (Durchmesser des Rades des Roboters)
- d_ball = 102 mm (tatsächlicher Kugeldurchmesser)
Als Ausgabeparameter dieser Aufgabe wurde ein Fehler ermittelt, der die Genauigkeit unserer Methode und der ausgewählten Parameter angibt:- error = abs (d_ball_exper - d_ball)
Programmbetriebsalgorithmus
Die folgenden Stufen des Algorithmus wurden identifiziert.- Bewegung zum Objekt.
- Zum rechten Punkt eines Objekts drehen.
- Drehen Sie zum äußersten linken Punkt des Objekts.
- Berechnung des Winkels anhand von Messwerten von Encodern.
- Fehlerberechnung.
RobotC-Implementierung des Algorithmus
Zunächst müssen wir die Standardkonfiguration des virtuellen Roboters initialisieren:#pragma config(StandardModel, "VEX Launchbot")
Wir implementieren eine Funktion, die dafür verantwortlich ist, den Roboter ganz nach rechts und / oder ganz nach links zu drehen. Die Funktion gibt den Encoder nach einer Drehung zurück, die für weitere Berechnungen verwendet wird:int turn_to_over_object(int leftSpeed, int rightSpeed, int distance, bool need_object, bool left_side)
{
if(need_object == true)
{
if(left_side == true)
{
while(SensorValue[sonarSensor] > distance)
{
setMultipleMotors(-leftSpeed, leftMotor,frontLeftMotor);
setMultipleMotors(rightSpeed, rightMotor,frontRightMotor);
}
}
else
{
while(SensorValue[sonarSensor] > distance)
{
setMultipleMotors(leftSpeed, leftMotor,frontLeftMotor);
setMultipleMotors(-rightSpeed, rightMotor,frontRightMotor);
}
}
}
else
{
if(left_side == true)
{
while(SensorValue[sonarSensor] < (distance + 1))
{
setMultipleMotors(-leftSpeed, leftMotor,frontLeftMotor);
setMultipleMotors(rightSpeed, rightMotor,frontRightMotor);
}
}
else
{
while(SensorValue[sonarSensor] < (distance + 1))
{
setMultipleMotors(leftSpeed, leftMotor,frontLeftMotor);
setMultipleMotors(-rightSpeed, rightMotor,frontRightMotor);
}
}
}
return getMotorEncoder(leftMotor);
}
Schauen wir uns die Funktionsargumente genauer an:- leftSpeed - Geschwindigkeit des linken Antriebs
- rightSpeed - richtige Fahrgeschwindigkeit
- rightSpeed - richtige Fahrgeschwindigkeit
- Entfernung - Entfernung zu dem Objekt, an dem der Roboter angehalten hat
- need_object - Bewegungsmodus: True, wenn ein Objekt im Sichtfeld des Sensors benötigt wird, False, wenn ein Objekt benötigt wird
- left_side - Drehrichtung des Roboters. Richtig - links abbiegen, Falsch - rechts abbiegen
Mit dem Wert need_object = True dreht sich der Roboter, bis sich das Objekt in einem Abstand vom Roboter befindet (in unserem Fall 100 mm).Mit dem Wert need_object = False dreht sich der Roboter, bis sich das Objekt in einer Entfernung (Entfernung + 1) vom Roboter befindet. Der Einheitsparameter insgesamt beeinflusst die endgültige Genauigkeit der Bestimmung des Fehlens eines Objekts im Sichtfeld des Sensors (es ist möglich, das Programm zu verbessern, indem dieser Parameter in Funktionsargumente aufgenommen wird).Schreiben wir den Hauptteil des Programms:task main()
{
int d_ball = 10.2; //sm
int d_wheel = 10.2;
int dist = 10; //sm
//
setMotor(armMotor, -50);
wait(1);
while(SensorValue[sonarSensor] > 10)
{
setMultipleMotors(50, leftMotor, rightMotor, frontLeftMotor, frontRightMotor);
}
stopAllMotors();
int enc = 0;
enc = turn_to_over_object(50, 50, dist, false, false);
enc = turn_to_over_object(50, 50, dist, true, true);
resetMotorEncoder(leftMotor);
enc = turn_to_over_object(50, 50, dist, false, true);
stopAllMotors();
float teta = abs(enc) * PI * d_wheel * d_wheel / (dist * 627);
float d_ball_exper = 2 * dist * sin(teta / 2);
float error = abs(d_ball_exper - d_ball);
while(1)
{
}
}
Die erste while-Schleife läuft, bis der Roboter den angegebenen Abstand zum Objekt erreicht hat. Als nächstes müssen wir die oben beschriebene Funktion einzeln aufrufen, um den Encoderwert zwischen der Position ganz rechts und ganz links zu messen (beachten Sie, dass der Encoder vor dem Messen auf Null zurückgesetzt werden muss).Um die Winkelformel zu erhalten, wurde ich von den folgenden Formeln geleitet:- enc = N * 627, wobei N die Anzahl der Umdrehungen des Rades ist, enc der Encoderwert ist, 627 die Encoderauflösung ist => N ausdrücken;- S = N * l, wobei l die Länge des Rades ist, l = PI * d_rad * d_rad => N ersetzen und l => S = enc * PI * d_rad * d_rad / 627;- andererseits S = dist * teta, wobei S ist die Größe des Bogens, den der Roboter gedreht hat, Teta ist der gewünschte zentrale Winkel;- Basierend auf den obigen Daten drücken wir teta => teta = enc * PI * d_wheel * d_wheel / (627 * dist) aus.Um den Akkord zu finden, der dem gewünschten Durchmesser des Balls entspricht, verwenden wir die Formel: d_ball_exper = 2 * dist * sin (teta / 2)Die Endlos-while-Schleife am Ende des Programms ist erforderlich, um die empfangenen Werte im Debugger anzuzeigen.Zusammenfassungsdaten
Die folgenden Werte des Kugeldurchmessers und der Fehler wurden auf dem virtuellen Feld erhalten: d_ball_exper = 10,6 cm Fehler = 0,4 cmWie Sie sehen können, ergibt die erhaltene Methode zum Messen der Breite einen kleinen Fehlerwert, der für die Genauigkeitsklasse der verwendeten Sensoren akzeptabel ist. Der Fehlerwert kann reduziert werden, indem der Wert des dist-Parameters ausgewählt wird (z. B. Starten eines Zyklus, bei dem sich der Roboter mit einem bestimmten Schritt zurückbewegt, der sich zu dist ändert) oder der PID-Regler verwendet wird, wodurch der Einfluss der Trägheit des Roboters auf den Endwert des Encoders während des Bremsens verringert wird.Perspektiven
In Zukunft werde ich andere Methoden zur Verwendung von Robotersensoren zur Bestimmung von Umgebungsparametern in Betracht ziehen.