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

# Connect to RoboDK
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 iterate movement with varying joint speed
def move_with_variable_speed(iterations, start_speed, end_speed):
    # Calculate the increment in joint speed per iteration
    speed_increment = (end_speed - start_speed) / (iterations - 1)

    for i in range(iterations):
        # Calculate the current joint speed
        joint_speed = start_speed + i * speed_increment
        linear_speed = 100  # Constant linear speed for all iterations (you can modify this)
        accel = joint_speed / 2  # Set acceleration proportional to joint speed

        # Set the speed and acceleration
        robot.setSpeed(linear_speed, joint_speed)
        robot.setAcceleration(accel)

        # Display speed settings
        print(f"Iteration {i+1}: Joint Speed={joint_speed:.2f} deg/s, Linear Speed={linear_speed} mm/s, Accel={accel:.2f}")

        # Move to T1
        robot.MoveJ(target_t1)

        # Return to Home
        robot.MoveJ(home)

# Perform 5 iterations ranging joint speed from 50 deg/s to 100 deg/s
move_with_variable_speed(5, 50, 100)