import serial
import time

i = 0 # Iteration counter

# Function to calculate the checksum of a Dynamixel packet
def calculate_checksum(packet):
    checksum = ~(sum(packet) % 256) & 0xFF
    return checksum

# Function to build the instruction packet for a Dynamixel AX-12A servo
def build_packet(id, instruction, parameters):
    packet = [0xFF, 0xFF, id, len(parameters) + 2, instruction] + parameters
    packet.append(calculate_checksum(packet[2:]))  # Calculate and add the checksum
    return bytearray(packet)

# Function to send a command to a Dynamixel servo
def send_command(ser, id, instruction, parameters=[]):
    packet = build_packet(id, instruction, parameters)
    ser.write(packet)
    time.sleep(0.01)  # Wait a short time for the servo to process the command

# Function to set the position of a servo
def set_position(ser, id, position):
    pos = [position & 0xFF, (position >> 8) & 0xFF]  # Convert position to 2 bytes (little endian)
    send_command(ser, id, 0x03, [0x1E] + pos)  # 0x1E is the position register for AX-12A

def set_speed(ser, id, val):
    speed = [val & 0xFF, (val >> 8) & 0xFF]  # Convert speed to 2 bytes (little endian)
    send_command(ser, id, 0x03, [0x20] + speed)

def set_LED(ser, id, val):
    LED = [val]
    send_command(ser, id, 0x03, [0x19] + LED)


# Open serial port (update the port name to match your setup)
with serial.Serial('/dev/ttyUSB0', baudrate=1000000, timeout=1) as ser:

    '''
    set_LED(ser, 1, 1) #turn on all ID's LED
    set_LED(ser, 2, 1)
    set_LED(ser, 3, 1)
    set_LED(ser, 4, 1)
    '''

    set_speed(ser, 1, 150) # Set move speed to val * 0.111 rpm (100*0.111 = 11.1 rpm)
    set_speed(ser, 2, 100)
    set_speed(ser, 3, 100)
    set_speed(ser, 4, 150)

    time.sleep(5) # Init timer

    # Set start pose in deg * 3.41 steps per deg
    set_position(ser, 1, int(150 * 3.41))  
    set_position(ser, 2, int(160 * 3.41))  
    set_position(ser, 3, int(160 * 3.41))  
    set_position(ser, 4, int(150 * 3.41))  
    time.sleep(5)

    while 1: 
        # Walking motion descided by manual angle, speed and waiting inputs. 
        # step 1 left foot forward
        # Servo 1 holds pos
        set_position(ser, 4, int((150-80) * 3.41)) #left knee
        time.sleep(0.2) #0.5
        set_position(ser, 3, int((150-25) * 3.41)) #left hip
        time.sleep(0.2) #0.5
        set_position(ser, 2, int((150-0) * 3.41)) #right hip
        time.sleep(0.2) #0.5
        set_position(ser, 2, int((150-10) * 3.41)) #right hip
        set_position(ser, 4, int((150-0) * 3.41)) #left knee
        time.sleep(0.2) #0.5
        set_position(ser, 3, int((150-10) * 3.41)) #left hip
        time.sleep(0.4) #0.8

        # step 2 right foot forward
        # servo 2 holds pos
        set_position(ser, 1, int((150+80) * 3.41)) #right knee
        time.sleep(0.2) #0.5
        set_position(ser, 2, int((150+25) * 3.41))  #right hip
        time.sleep(0.2) #0.5
        set_position(ser, 3, int((150+0) * 3.41))  #left hip
        time.sleep(0.2) #0.5
        set_position(ser, 3, int((150+10) * 3.41))  #left hip
        set_position(ser, 1, int((150+0) * 3.41)) #right knee
        time.sleep(0.2) #0.5
        set_position(ser, 2, int((150+10) * 3.41))  #right hip
        time.sleep(0.4) #0.8

    # print("Commands sent to servos")