from dynamixel_sdk import * # Import Dynamixel SDK library import time # Control Table Address for AX-12A ADDR_MX_TORQUE_ENABLE = 24 ADDR_MX_GOAL_POSITION = 30 ADDR_MX_PRESENT_POSITION = 36 ADDR_MX_MOVING_SPEED = 32 ADDR_MX_TORQUE_LIMIT = 34 MAX_TORQUE = 1000 # Protocol version for AX-12A PROTOCOL_VERSION = 1.0 # Default setting values DEVICENAME = '/dev/ttyUSB0' # Check your port (it may be COM3, /dev/ttyUSB0, etc.) BAUDRATE = 1000000 TORQUE_ENABLE = 1 TORQUE_DISABLE = 0 DXL_MOVING_STATUS_THRESHOLD = 10 # Dynamixel moving status threshold # Initialize PortHandler and PacketHandler portHandler = PortHandler(DEVICENAME) packetHandler = PacketHandler(PROTOCOL_VERSION) # Open port and set baudrate if portHandler.openPort(): print("Port opened successfully") else: print("Failed to open port") if portHandler.setBaudRate(BAUDRATE): print("Baudrate set to", BAUDRATE) else: print("Failed to set baudrate") # Servo IDs (adjust to your configuration) ANKLE_RIGHT_ID = 0 KNEE_RIGHT_ID = 1 HIP_RIGHT_ID = 2 HIP_LEFT_ID = 3 KNEE_LEFT_ID = 4 ANKLE_LEFT_ID = 5 # Enable torque for each servo def enable_torque(servo_id): packetHandler.write1ByteTxRx(portHandler, servo_id, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE) # Set goal position for each servo def set_goal_position(servo_id, position): packetHandler.write2ByteTxRx(portHandler, servo_id, ADDR_MX_GOAL_POSITION, position) def set_torque_limit(servo_id, torque_limit=MAX_TORQUE): packetHandler.write2ByteTxRx(portHandler, servo_id, ADDR_MX_TORQUE_LIMIT, torque_limit) for servo_id in [ANKLE_RIGHT_ID, KNEE_RIGHT_ID, HIP_RIGHT_ID, HIP_LEFT_ID, KNEE_LEFT_ID, ANKLE_LEFT_ID]: set_torque_limit(servo_id) # Walking gait pattern (adjust values based on your robot's range of motion) def reset_to_neutral(): set_goal_position(HIP_LEFT_ID, 512) set_goal_position(KNEE_LEFT_ID, 512) set_goal_position(ANKLE_LEFT_ID, 512) set_goal_position(HIP_RIGHT_ID, 512) set_goal_position(KNEE_RIGHT_ID, 512) set_goal_position(ANKLE_RIGHT_ID, 512) time.sleep(0.1) def walking_step(): # Start with a neutral position for smooth transition #reset_to_neutral() # Left leg step forward set_goal_position(HIP_LEFT_ID, 750) # Gradual forward swing of left hip set_goal_position(KNEE_LEFT_ID, 400) # Slight bend in left knee set_goal_position(ANKLE_LEFT_ID, 600) # Heel contact with ground set_goal_position(HIP_RIGHT_ID, 574) # Right hip shifts backward set_goal_position(KNEE_RIGHT_ID, 512) # Right knee straight set_goal_position(ANKLE_RIGHT_ID, 474) # Right ankle raised time.sleep(0.1) # Smooth transition delay # Left leg moves back to neutral as right leg prepares to step set_goal_position(HIP_LEFT_ID, 512) # Left hip back to neutral set_goal_position(KNEE_LEFT_ID, 512) # Left knee straightens set_goal_position(ANKLE_LEFT_ID, 512) # Left ankle flat set_goal_position(HIP_RIGHT_ID, 374) # Right hip forward for next step set_goal_position(KNEE_RIGHT_ID, 624) # Right knee bend for step set_goal_position(ANKLE_RIGHT_ID, 574) # Heel contact with ground time.sleep(0.1) # Right leg step forward set_goal_position(HIP_LEFT_ID, 512) # Left hip shifts backward set_goal_position(KNEE_LEFT_ID, 512) # Left knee straight set_goal_position(ANKLE_LEFT_ID, 550) # Left ankle raised set_goal_position(HIP_RIGHT_ID, 354) # Right hip forward swing set_goal_position(KNEE_RIGHT_ID, 624) # Right knee bend for step set_goal_position(ANKLE_RIGHT_ID, 574) # Heel contact with ground time.sleep(0.1) # Right leg moves back to neutral as left leg prepares to step set_goal_position(HIP_LEFT_ID, 750) # Left hip forward for next step set_goal_position(KNEE_LEFT_ID, 400) # Left knee bend for step set_goal_position(ANKLE_LEFT_ID, 600) # Heel contact with ground set_goal_position(HIP_RIGHT_ID, 512) # Right hip back to neutral set_goal_position(KNEE_RIGHT_ID, 512) # Right knee straight set_goal_position(ANKLE_RIGHT_ID, 512) # Right ankle flat time.sleep(0.1) # Enable torque for all servos enable_torque(HIP_LEFT_ID) enable_torque(KNEE_LEFT_ID) enable_torque(ANKLE_LEFT_ID) enable_torque(HIP_RIGHT_ID) enable_torque(KNEE_RIGHT_ID) enable_torque(ANKLE_RIGHT_ID) # Loop for continuous walking reset_to_neutral() time.sleep(0.8) try: while True: walking_step() except KeyboardInterrupt: reset_to_neutral() time.sleep(1) # Disable torque when stopping for servo_id in [HIP_LEFT_ID, KNEE_LEFT_ID, ANKLE_LEFT_ID, HIP_RIGHT_ID, KNEE_RIGHT_ID, ANKLE_RIGHT_ID]: packetHandler.write1ByteTxRx(portHandler, servo_id, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE) print("\nTorque disabled and program stopped.") # Close the port portHandler.closePort()