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()
Comments