from robodk.robolink import *  # RoboDK API
from robodk.robomath import *  # Math library for RoboDK

# Initialize RoboDK API
RDK = Robolink()

# Select the robot (replace with the exact robot name)
robot = RDK.Item('Omron TM12X', ITEM_TYPE_ROBOT)

# Ensure the robot is valid
if not robot.Valid():
    raise Exception("Robot not found in RoboDK!")

# Define the targets
home = RDK.Item('Home', ITEM_TYPE_TARGET)
target_t1 = RDK.Item('T1', ITEM_TYPE_TARGET)

if not home.Valid() or not target_t1.Valid():
    raise Exception("Home or T1 target not found in RoboDK!")

# Move to Home position
robot.MoveJ(home)

# Function to move to target T1 with varying speeds
def move_with_speed(iterations):
    for i in range(iterations):
        # Set random joint speeds for each iteration
        speed = 50 + i * 10  
        accel = speed / 2    

        # Set speed and acceleration in RoboDK
        robot.setSpeed(speed, accel)

        print(f"Iteration {i+1}: Moving to T1 with speed={speed} mm/s and accel={accel} mm/s^2")
        
        # Move to T1
        robot.MoveJ(target_t1)

        # Return to Home
        robot.MoveJ(home)

# Call the function with the desired number of iterations
move_with_speed(5)