from robolink import *
from robodk import *      # Robot toolbox
from tkinter import *     #
from threading import *   # Import Threading
import serial             # Import USB -connection

RDK = Robolink() # Sets Robolink to new variable RDK
robot = RDK.Item('KUKA KR 210 L150-2') # Assigning the item, as our robot
robot.setJoints([0, 0, 0, 0, 0, 0]) # Setting the robot to default position
arduinoData = serial.Serial('COM5',9600) # Gathering data from the serial port
newData = bytes(arduinoData.read()) #Sets the data readable from the usb
decodedData = newData.decode("utf-8")

#Defining 4 test targets
target1 =RDK.Item('target 1')
target2 =RDK.Item('target 2')
target3 =RDK.Item('target 3')
target4 =RDK.Item('target 4')
crash = False
def sensor_check(): # Creating a function that controll the sensor
    print(decodedData)
    while decodedData.__contains__("1"):
            print("Contains 1")
            print(newData)
            robot.Pause(500) #Stops the robot
            crash = True

def robot_move(): # The robot starts to move in a while loop
    while crash == False: # It stops if crash equals to true
        robot.moveJ(target1)
        robot.moveJ(target2)
        robot.moveJ(target3)
        robot.moveJ(target4)
sensor_check() # Intialize sensorcheck
robot_move() # Intaliaze robot movement