import serial
import struct
import time  # Added import for time module

# Define communication constants and machine information
BAUD_RATE = 38400
DATA_BITS = 8
STOP_BITS = 1
PARITY = serial.PARITY_NONE
NODE_ID = 0x01

# Define commands for motor control
SET_CONTROLWORD_CMD = 0x2B
SET_OPERATION_MODE_CMD = 0x2F
SET_TARGET_POSITION_CMD = 0x41
READ_STATUSWORD_CMD = 0x4B
SET_PROFILE_SPEED_CMD = 0x23

# Define functions to calculate checksum and send commands
def calculate_checksum(data):
    return (-sum(data) % 256) & 0xFF

def send_command(serial_port, cmd, index, sub_index, data=None):
    if data is None:
        data = b''
    packet = bytes([NODE_ID, cmd, index & 0xFF, index >> 8, sub_index, *data])
    checksum = calculate_checksum(packet)
    packet += bytes([checksum])
    serial_port.write(packet)
    response = serial_port.read(8)
    return response

# Motor control functions
def move_to_position(serial_port, position):
    response = send_command(serial_port, SET_TARGET_POSITION_CMD, 0x607A, 0x00, data=list(struct.pack('<i', position)))
    print(f"Response (Move to Position {position}):", response.hex())

def set_speed(serial_port, speed):
    speed_bytes = struct.pack('<i', speed)
    response = send_command(serial_port, SET_PROFILE_SPEED_CMD, 0x6081, 0x00, data=list(speed_bytes))
    print(f"Response (Set Speed {speed}):", response.hex())

def start_motion(serial_port):
    response = send_command(serial_port, SET_CONTROLWORD_CMD, 0x6040, 0x00, data=[0x0F, 0x00, 0x00, 0x00])  # Start motion
    print("Response (Start motion):", response.hex())

def stop_motion(serial_port):
    response = send_command(serial_port, SET_CONTROLWORD_CMD, 0x6040, 0x00, data=[0x06, 0x00, 0x00, 0x00])  # Stop motion
    print("Response (Stop motion):", response.hex())

def move_to_positions(serial_port, positions):
    for pos in positions:
        move_to_position(serial_port, pos)
        time.sleep(1)  # Added delay to allow time for motion
        set_speed(serial_port, 200)  # Set speed to 200 RPM
        start_motion(serial_port)
        time.sleep(5)  # Added delay to simulate motion time
        stop_motion(serial_port)

# Main function
def main():
    serial_port = serial.Serial('COM3', baudrate=BAUD_RATE, bytesize=DATA_BITS, stopbits=STOP_BITS, parity=PARITY, timeout=1)

    try:
        positions = [5000001, 6000001, 7000001]
        move_to_positions(serial_port, positions)

    finally:
        serial_port.close()

if __name__ == "__main__":
    main()