Realtime G-code

Die Datei GCodeSender.py ist wie eine normale Library geschrieben.
Zuerst muss man sie importieren und ein Objekt instanziieren (Achtung: Es wird mit dem Roboterarm verbunden und eine Nullpunktfahrt ausgeführt!).
Um GCodeSender.py zu importieren :

from gcodesender import *
gcsender = GCodeSender('COMX') # oder '/dev/ttyUSBX'  -> X = Port der Roboterarmes

Es gibt folgende Funktionen:

Die Zeit in Sekunden bis der Arm von Punkt A nach Punkt B fährt ergibt sich wie folgt:

rel_x = x - self.last_x  # calc relative change in position from last position
rel_y = y - self.last_y
rel_z = z - self.last_z
t = math.sqrt(rel_x * rel_x + rel_y * rel_y + rel_z * rel_z) * (60 / f) * 0.9  # 3D Pythagoras of x,y,z * feedrate in sec/mm * calibration
self.last_x = x  # update last position to current position
self.last_y = y
self.last_z = z

Der Code behält immer die letzte Position des Armes im Speicher, um die Zeit auszurechnen.
Indem gewartet wird bis der Arm den aktuellen Move abgeschlossen hat, und erst dann ein neuer Move gesendet wird, wird verhindert, dass sich der serielle Buffer im Arduino füllt. So wird immer erst ein Move gesendet, wenn der Arm diesen direkt ausführen kann. Der aktuelle Move basiert so immer auf den aller neusten Daten aus der Object-Recognition mit YOLOv5.

Zum Sprühen des Herbizids wird eine Pumpe verwendet.

Das Kommando M106 wird zur Steuerung der Herbizidpumpe verwendet. Mit diesem G-Code wird normalerweise der Ausgang des Lüfters ein- oder ausgeschaltet. Die enablePump() Funktion schaltet den 12V Ausgang “D9” ein. S0 bzw. S255 bedeutet Geschwindigkeit, und die von uns angegebene 255 ist die maximale Geschwindigkeit. Die Funktion disablePump() schaltet den Ausgang ab.

def enablePump(self):
    # turn the pump on (D9 on Ramps PCB)
    # set the pump to 100% power. 255 is full speed (12V)
    self.ser.write(b'M106 S255 \r\n')
    self.ser.flush()
 
def disablePump(self):
    # turn off pump completely. 0 is no speed
    self.ser.write(b'M106 S0 \r\n')
    self.ser.flush()

Um den Delta Arm mit Objekterkennung zu testen, wurde OpenCV verwendet. Das Skript erkennt grüne Objekte und weist den Drucker an, die Position des Objektes anzufahren. Es funktioniert wie folgt:

• Durch eine Kamera ist ein grünes Objekt erkannt werden
• Das oben erfasste Bild laden, Farbräume und Glättung ändern
• die Kontur des Objekts finden und die Kontur zeichnen. Hier werden die XY-Positionen bestimmt. Außerdem wird der Mittelpunkt der Positionen berechnet
• G-Code mit der Position des Objekts senden

Für die Entwicklung der Objekterkennung wird OpenCV verwendet, eine Bibliothek für Bildverarbeitung / Bildanalyse.

Der Kommando G1 wird verwendet, um die Koordinaten für die lineare Bewegung zu senden.

1. Ein grünes Objekt

2. Erkennung eines Objekts. Die Konsole zeigt die für den Vorgang benötigte Zeit an.

3. Oben ist das Bild des erkannten Objekts mit dem Begrenzungsrahmen und dem Mittelpunkt des Objekts nach der Erfassung der Koordinaten dargestellt. Das Bild unten zeigt das erkannte Objekt nach der Entrauschung. Um das erkannte Objekt zu analysieren, wurde es in ein Schwarz-Weiß-Bild umgewandelt.

opencv wird verwendet, um ein bestimmtes Objekt (in diesem Fall ein grünes Objekt) aus dem Video zu erkennen und die Koordinaten des Objekts zu erhalten. Um ein bewegtes Objekt zu erkennen, verwenden wir das Video anstelle des Bildes. Um den Delta-Arm in die Mitte des Objekts zu bewegen, verwenden wir eine Bounding-Box, um die Koordinaten des erkannten Objekts zu erhalten, und rufen gcsender auf, um den Gcode zu senden.

import cv2
import serial
import time
import imutils
import math
from gcodesender import *
 
# detect only green obj
greenLower = (29, 86, 6)
greenUpper = (64, 255, 255)
 
feedrate = 4200 # mm/min   #  4300 mm/min is too fast, and the robot arm cant keep up
z_pos = 170  
nextCommandTime = 0
 
camera = cv2.VideoCapture(0)
 
gcsender = GCodeSender('COM7') # choose your own port
 
while True:
    ret, frame = camera.read()
 
    # frame = imutils.resize(frame, width=600, height=300)
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
 
    mask = cv2.inRange(hsv, greenLower, greenUpper)
    mask = cv2.erode(mask, None, iterations=2)
    mask = cv2.dilate(mask, None, iterations=2)
 
    cnt, hierarchy = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2:]
    if nextCommandTime < time.time():
        for c in range(len(cnt)):
            # determine positions + bounding box
            x, y, w, h = cv2.boundingRect(cnt[c])
            cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255, 0), 2)
 
            # center positions
            x2 = int(x + w / 2)
            y2 = int(y + h / 2)
 
            # send G-Code and capture time needed for operation
            timeToWait = gcsender.send(x2, y2, z_pos, feedrate)
            # calculate next time a command can be issued
            nextCommandTime = time.time() + timeToWait
            print('timetowait: ' + str(timeToWait) + '\n')
 
 
            # draw center in bounding box
            cv2.circle(frame, (x2, y2), 4, (0, 255, 0), -1)
 
    cv2.imshow("Frame", frame)
    cv2.imshow("Mask", mask)
 
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break
 
gcsender.cleanup()
camera.release()
cv2.destroyAllWindows()