SCS0009 test

scs0009_test.py

import time
import serial
import struct
from typing import Tuple

s = serial.Serial('COM19', 1000000)
def read_packet(servo_id: int, reg_address: int, reg_length: int) -> Tuple[bytes, int]:
    packet = [0xff, 0xff, servo_id, 0x04, 0x02, reg_address, reg_length]
    packet.append(0xff - (sum(packet[2:]) & 0xff))  # checksum
    response_length = 2 + 1 + 1 + 1 + reg_length + 1 # header, servo_id, packet_length, command, content, checksum
    return (bytes(packet), response_length)
def write_packet(servo_id: int, reg_address: int, data: bytes) -> bytes:
    packet_header = [0xff, 0xff, servo_id, len(data) + 3, 0x03, reg_address]
    checksum = sum(packet_header[2:]) + sum(data)
    checksum = 0xff - (checksum & 0xff)
    return bytes(packet_header) + data + bytes([checksum])

servo_id = 0x01

with s:

    packet, response_length = read_packet(servo_id, 0x03, 10)
    print(packet)
    s.write(bytes(packet))
    response = s.read(len(packet) + response_length)[len(packet):]
    print(response)
    sw_ver, id, baud, delay, status, lower_angle, upper_angle = struct.unpack_from('>hbbbbhh', response[5:])
    print(sw_ver, id, baud, delay, status, lower_angle, upper_angle)

    packet = write_packet(servo_id, 0x2a, struct.pack('>h', 0x0020))
    print(packet)
    s.write(packet)

    packet = write_packet(servo_id, 0x28, b'\x01')
    print(packet)
    s.write(packet)

    while True:
        packet = write_packet(servo_id, 0x2a, struct.pack('>h', lower_angle))
        print(packet)
        s.write(packet)
        time.sleep(5)
        
        packet = write_packet(servo_id, 0x2a, struct.pack('>h', upper_angle))
        print(packet)
        s.write(packet)
        time.sleep(5)