# Indicator

import serial #Serial port API


#Connection parameters for port 21

ser = serial.Serial(port='COM1', baudrate=19200, bytesize=7, parity=serial.PARITY_EVEN, stopbits=1, timeout=1)

 

from robolink import *    # RoboDK API

from robodk import *      # Robot toolbox

RDK = Robolink()

 

robot = RDK.Item('KUKA KR 6 R700 sixx', ITEM_TYPE_ROBOT) # Get the robot by name

RDK.setRunMode(RUNMODE_RUN_ROBOT)

#RDK.setRunMode(RUNMODE_SIMULATE)

tool  = RDK.Item('Tool 1')

robot.setPoseTool(tool)

print(tool)

 

 

#How do I limit angular speed????

robot.setSpeed(10, 3, 1, 1)

#Why doesn't the following line work???

robot.RunInstruction('SetCartAngVel(2)\n', INSTRUCTION_INSERT_CODE)

 

# go to start position

robot.MoveL([3.874, -8.011, 23.15, -5.476, 30.04, 7.486])

robot.MoveL([3.874, -7.95, 29.178, -6.749, 23.985, 8.912])

 

# Move the robot cycle

def AutoZero():

    indicator = 1

    while abs(float(indicator)) > 0.005:

        ser.flushInput() #Remove data from input buffer

        ser.flushOutput() #Remove data from output buffer

        ser.write("%01#RDD0010000107**\r".encode()) #Send request to the indicator

        read=str(ser.readline()) #Read the indicator

        indicator=(read[2:][:-3]) #Remove unnecessary characters from the string

        print(float(indicator))#Print the values

        current_EE_pose = robot.Pose()

        new_EE_pose = Offset(current_EE_pose, x=0, y=0, z=float(indicator))

        robot.MoveL(new_EE_pose)

 

AutoZero()

initial_EE_pose = robot.Pose()

robot.MoveL(RelTool(initial_EE_pose,x=0,y=0,z=0,rx=40,ry=0,rz=0))

robot.WaitMove(1)

AutoZero()

robot.MoveL(initial_EE_pose)

robot.MoveL(RelTool(initial_EE_pose ,x=0,y=0,z=0,rx=-40,ry=0,rz=0))

robot.WaitMove(1)

AutoZero()

robot.MoveL(initial_EE_pose)

robot.MoveL(RelTool(initial_EE_pose ,x=0,y=0,z=0,rx=0,ry=40,rz=0))

robot.WaitMove(1)

AutoZero()

robot.MoveL(initial_EE_pose)

robot.MoveL(Offset(initial_EE_pose ,x=0,y=0,z=10,rx=0,ry=0,rz=0))