====== Realtime G-code ======
===== Code =====

<WRAP download>
G-Code Sender mit Beispielen: https://gitlab.com/Romanizer/gcode-py/-/tree/master/gcode\\
</WRAP>
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 :
<file python>
from gcodesender import *
gcsender = GCodeSender('COMX') # oder '/dev/ttyUSBX'  -> X = Port der Roboterarmes
</file>

Es gibt folgende Funktionen:
^ Funktion ^ Erklärung ^  Parameter ^ Rückgabewert ^
| gcsender.send(x,y,z,f) | sendet den Arm an die gegebene Position | x,y,z: absolute Position, f: feedrate | Zeit in Sekunden bis der Move abgeschlossen ist | 
| gcsender.enablePump()  | sendet ''M106 S255'' und die Pumpe geht an | - | - |
| gcsender.disablePump() | sendet ''M106 S0'' und die Pumpe geht aus | - | - |
| gcsender.cleanup()     | schliesst die serielle Verbindung zum Arm | - | - | 

Die Zeit in Sekunden bis der Arm von Punkt A nach Punkt B fährt ergibt sich wie folgt:\\
<file python>
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
</file>
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.

==== Pumpe ====
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.

<file python>
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()
</file>

===== Tests =====
=== Allgemeine Informationen ===
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

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

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

^ Parameter ^ Bereich ^ Erklärung ^
| X      | -100, +100 |  Links und Rechts |
| Y      | -100, +100 |  Vorne und Hinten |
| Z      | 0, +245    |  Höhe (+ is up) |
| F      | <4200  (firmware limit)     |  Feedrate in mm/min |

=== Bilder ===
1. Ein grünes Objekt\\ 
{{:projects:farmrobot:testwithopencv-2.jpg?400|}}\\ 
2. Erkennung eines Objekts. Die Konsole zeigt die für den Vorgang benötigte Zeit an.\\ 
{{:projects:farmrobot:testwithopencv-1.jpg?400|}}\\ 
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.\\ 
{{:projects:farmrobot:testwithopencv-3.jpg?400|}}

=== Code block ===
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.

<file python>
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()
</file>