Mit diesen beiden Code-Beispielen wird gezeigt, wie man direkt auf dem OpenMV Kameraboard die Analyse der Bilddaten und auch weitere Berechungen ausführen kann, um Rechenkapazitäten vom Mikrocontroller eines KeplerBRAIN Roboters zur OpemMV Cam zu verlagern. Im Programm des KeplerBRAIN Roboters können diese Werte dann verwendet werden, ohne, dass dort Rechenleistung und Speicherplatz für Programmcode und Variablen verbraucht wird. Hier wird der Abweichungswinkel (der in der OpenMV Cam berechnet wird) verwendet, um die beiden Motorgeschwindigkeiten laufend so zu berechnen und zu aktualisieren, dass der Roboter immer in Richtung der Mitte der schwarzen Linie hin korrigiert.
Es handelt sich hierbei um eine klassische proportionale Fehlerkorrektur, bei welcher der Fehler (die Abweichung von der schwarzen Linie) mit einer festgelegten Gewichtung in die Berechnung der Motorgeschwindigkeiten einfließt. Ausgehend von einer festgelegten Startgeschwindigkeit der beiden Motoren wird jeweils der Fehler, multipliziert mit einem Proportionalitätsfaktor P dazugezählt oder abgezogen.
M1 speed aktuell = M1 speed start + Fehler * P
M2 speed aktuell = M2 speed start - Fehler * P
Python Skript zur Berechnung des Abweichungswinkels
import pyb, sensor, image, time, math# ******************** SPI SEND INTERUPT ********************spi = pyb.SPI(2, pyb.SPI.SLAVE, polarity=0, phase=0)led_red = pyb.LED(1)led_green = pyb.LED(2)spi_list = [250, 0, 0, 0, 0, 0, 0, 0]spi_data = bytearray(spi_list)def nss_callback(line):global spi, spi_datatry:spi.send(spi_data, timeout=1000)led_green.on()led_red.off()except OSError as err:led_green.off()led_red.on()passpyb.ExtInt(pyb.Pin("P3"), pyb.ExtInt.IRQ_FALLING, pyb.Pin.PULL_UP, nss_callback)# ******************** IMAGE DETECTION ********************GRAYSCALE_THRESHOLD = [(0, 64)]ROIS = [(0, 100, 160, 20, 0.7),(0, 50, 160, 20, 0.3),(0, 0, 160, 20, 0.1)]weight_sum = 0for r in ROIS: weight_sum += r[4]sensor.reset()sensor.set_pixformat(sensor.GRAYSCALE)sensor.set_framesize(sensor.QQVGA)sensor.set_vflip(True)sensor.set_hmirror(True)sensor.skip_frames(time = 2000)sensor.set_auto_gain(False)sensor.set_auto_whitebal(False)clock = time.clock()while(True):# ***** IMAGE DETECTION CODE *****clock.tick()img = sensor.snapshot()centroid_sum = 0for r in ROIS:blobs = img.find_blobs(GRAYSCALE_THRESHOLD, roi=r[0:4], merge=True)if blobs:largest_blob = max(blobs, key=lambda b: b.pixels())img.draw_rectangle(largest_blob.rect())img.draw_cross(largest_blob.cx(), largest_blob.cy())centroid_sum += largest_blob.cx() * r[4]center_pos = (centroid_sum / weight_sum) # Determine center of line.deflection_angle = 0deflection_angle = -math.atan((center_pos-80)/60)deflection_angle = math.degrees(deflection_angle)# ***** SET and SEND VALUES over SPI to ARDUINO *****spi_list[1]=int(deflection_angle)+100spi_data = bytearray(spi_list)# print("Turn Angle: %f" % deflection_angle)# print("FPS: " + str(clock.fps()))# print(spi_list)
Arduino Sketch zur Berechnung von Motorgeschwindigkeiten unter Verwendung des Abweichungswinkel, der von einer OpenMV Cam ermittelt wurde
ACHTUNG: Im folgenden Beispiel sind die Platzhalter ML und MR für die jeweiligen Motorausgänge des verwendeten Roboters zu ersetzen!
#include "KeplerBRAIN_V4.h"uint8_t value_1;uint8_t value_2;uint8_t value_3;uint8_t value_4;uint8_t value_5;uint8_t value_6;uint8_t value_7;float motorl_offset;float motorr_offset;float motorl_speed;float motorr_speed;float kp;int line_error;void setup(){KEPLERBRAIN_INIT();WRITE_LCD_TEXT(1,1," ");WRITE_LCD_TEXT(1,2," ");motorl_offset = 20;motorr_offset = 20;kp = 1;}void loop(){// read 8 Bytes from OpenMV BEGINdigitalWrite(SPICAM, LOW);delay(1);if(spi_cam.transfer(1) == 250){value_1 = spi_cam.transfer(0);value_2 = spi_cam.transfer(0);value_3 = spi_cam.transfer(0);value_4 = spi_cam.transfer(0);value_5 = spi_cam.transfer(0);value_6 = spi_cam.transfer(0);value_7 = spi_cam.transfer(0);}digitalWrite(SPICAM, HIGH);// read 8 Bytes from OpenMV ENDline_error = value_1 - 100;motorl_speed = motorl_offset - kp * line_error;motorr_speed = motorr_offset + kp * line_error;WRITE_MOTOR(ML,(int)motorl_speed);WRITE_MOTOR(MR,(int)motorr_speed);WRITE_LCD_TEXT(1,1,"Angle: " + String(line_error);WRITE_LCD_TEXT(1,2,"ML: " + String((int)motorl_speed + " MR: " +String((int)motorr_speed ));}
Erklärungen zu diesem Programmcode
Zeile 11, 12: float motorl_offset; float motorr_offset;
Definiton der Variablen motorl_offset und motorr_offset vom Typ float, in welchen in der Folge die Grundgeschwindigkeit für beide Motoren festgelegt wird.
Zeile 13, 14: float motorl_speed; float motorr_speed;
Definiton der Variablen motorl_speed und motorr_speed vom Typ float. In diesen werden die aktuellen Geschwindigkeiten abgelegt, die unter Berücksichtigung des Abweichwinkels berechnet werden, damit der Roboter in Richtung des Verlaufs und der Mitte der schwarzen Linie korrigiert.
Zeile 15: float kp;
Definition der Variable kp vom Typ float. Darin wird ein Proportionalitätsfaktor gespeichert, der darüber bestimmt, wie stark der Wert des Abweichungswinkels in die Berechnug der aktuellen Motorgeschwindigkeiten einfließen soll.
Zeile 16: float line_error;
Definition der Variable line_error vom Typ float. Der Variable line_error wird der Wert des Abweichungswinkels zugewiesen. Dieser wird hier als Ausgangspunkt - also als Fehler bzw. Abweichung des Roboters von der schwarzen Linie - für die Berechnung der Motorgeschwindigkeiten herangezogen.
Zeile 24, 25: motorl_offset = 20; motorr_offset = 20;
Hier werden die Grundgeschwindigkeiten für die beiden Motoren mit dem Wert 20 festgelegt. Mit diesen Werten kann experimentiert werden, um ein bestmögliches Mittelmaß zwischen hoher Fahrgeschwindigket und großer Sicherheit beim Folgen der Linie (ohne diese zu verlieren!) zu finden.
Zeile 26: kp = 1;
Der Proportionalitätsfaktor wird mit dem Wert 1 festgelegt. Das bedeutet, dass der aktuelle Abweichungswinkel 1:1 zu den Motorgrundgeschwindigkeiten addiert oder subtrahiert wird (siehe Zeile 53 und 54).
Sind diese Korrekturen zu stark und verliert der Roboter die Linie, so kann ein Wert kleiner 1 gewählt werden. Sind die Korrekturen zu schwach und verliert der Roboter die Linie, kann ein Wert größer 1 gewählt werden.
Der optimale Wert ist experimentell zu bestimmen und steht selbstverständlich im Zusammenhang mit den in Zeile 15 und 16 festgelegten Grundgeschwindigkeiten der Motoren.
Zeile 53, 54: motorl_speed = motorl_offset - kp * line_error; motorr_speed = motorr_offset + kp * line_error;
Dies sind die zentralen Berechnungen für die Ermittlung der aktuellen Motorgeschwindigkeiten basierend auf dem aktuellen Fehler, der durch den von einer OpenMV Cam bestimmten Abweichungswinkel beschrieben wird. Die aktuelle Motorgeschwindigkeit wird berechnet, indem zur Grundgeschwindigkeit eines Motors ein Wert addiert oder subtrahiert wird. Dieser Wert ergibt sich aus dem Produkt des aktuellen Fehlers (Abweichungswinkel) und dem Proportionalitätsfaktor kp.
Zeile 56, 57 WRITE_MOTOR(ML,(int)motorl_speed); WRITE_MOTOR(MR,(int)motorr_speed);
Die Geschwindigkeiten für die beiden Motoren werden gesetzt. Dabei ist zu beachten, dass alle Berechnungen mit Dezimalzahlen durchgeführt werden und bei der Übergabe der Werte für die Geschwindigkeiten an die Funktion WRITE_MOTOR() diese zuerst mit (int) in ganzzahlige Werte konvertiert werden müssen.