#!/usr/bin/env python # -*- coding: utf-8 -*- import os import json import time # Read the sensors positions from the sensor_positions.json file with open('sensor_positions.json', 'r') as f: config = json.load(f) ########################################################################### ## SET THE NOTES TO PLAY AS SUCH: ## notes_to_play = [NOTE1, NOTE2,...,NOTEx] ## with the notes being ## C, C#, D, D#, E, F, F#, G, G#, A, A#, B ## = 1, 2 , 3, 4 , 5, 6, 7 , 8, 9 ,10, 11, 12 ## ## To play at rythm, the duration of a note must be added after the ID ## like so: notes_to_play = [NOTE1, NOTE2,'half', NOTE3, 'eigth',...,NOTEx] ## to play NOTE2 for longer and NOTE3 faster ########################################################################### #note_to_play = 2 #note id to play (see sensor_positions.json) #notes_to_play = [1,2,3,4,5,6,7,8,9,10,11,12] #play Au clair de la Lune #notes_to_play = [1, 1, 1, 3, 5,'half', 3,'half', 1, 5, 3, 3, 1, 'half'] #play Ode yo Joy #notes_to_play = [5, 5, 6, 8, 8, 6, 5, 3, 1, 1, 3, 5, 5, 3, 3, 'half', 5, 5, 6, 8, 8, 6, 5, 3, 1, 1, 3, 5, 3, 1, 1, 'half', 3, 3, 5, 1, 3, 5,'eigth', 6,'eigth', 5, 1, 3, 5,'eigth', 6,'eigth', 5, 3, 1, 3, 8, 'half', 5, 5, 6, 8, 8, 6, 5, 3, 1, 1, 3, 5, 3, 1, 1] #play Happy Birthday notes_to_play = [1, 1, 3, 1, 6, 5,'half', 1, 1, 3, 1, 8, 6,'half', 1, 1, 10, 8, 6, 5, 3,'eigth', 3,'half', 11, 11, 10, 6, 8, 6] #play megalovania #notes_to_play = [3, 3, 10, 9, 8, 6, 3, 6, 8] notes_to_play = [3,3, 5,3, 10 ,5 ,10,12,6,5,3] notes_to_play.append('end') #to know when it is the end of the array delay = 0.1 note_delay = 0.2 #delay between notes, corresponding to tempo init_note_delay = note_delay if os.name == 'nt': import msvcrt def getch(): return msvcrt.getch().decode() else: import sys, tty, termios fd = sys.stdin.fileno() old_settings = termios.tcgetattr(fd) def getch(): try: tty.setraw(sys.stdin.fileno()) ch = sys.stdin.read(1) finally: termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) return ch from dynamixel_sdk import * # Uses Dynamixel SDK library # Control table address ADDR_PRO_TORQUE_ENABLE = 24 # Control table address is different in Dynamixel model ADDR_PRO_GOAL_POSITION = 30 ADDR_PRO_PRESENT_POSITION = 37 # Protocol version PROTOCOL_VERSION = 2.0 # See which protocol version is used in the Dynamixel # Set Dynamixel motors IDs DXL1_ID = 1 DXL2_ID = 2 DXL3_ID = 3 BAUDRATE = 1000000 # Dynamixel baudrate : 1000000 DEVICENAME = 'COM10' # Check which port is being used on your controller # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" TORQUE_ENABLE = 1 # Value for enabling the torque TORQUE_DISABLE = 0 # Value for disabling the torque #CONNECT TO ROBOT # Initialize PortHandler instance # Set the port path # Get methods and members of PortHandlerLinux or PortHandlerWindows portHandler = PortHandler(DEVICENAME) # Initialize PacketHandler instance # Set the protocol version # Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler packetHandler = PacketHandler(PROTOCOL_VERSION) # Open port if portHandler.openPort(): print("Succeeded to open the port") else: print("Failed to open the port") print("Press any key to terminate...") getch() quit() # Set port baudrate if portHandler.setBaudRate(BAUDRATE): print("Succeeded to change the baudrate") else: print("Failed to change the baudrate") print("Press any key to terminate...") getch() quit() #ENABLE TORQUES # Enable Dynamixel#1 Torque dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE) if dxl_comm_result != COMM_SUCCESS: print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: print("%s" % packetHandler.getRxPacketError(dxl_error)) else: print("Dynamixel#%d has been successfully connected" % DXL1_ID) # Enable Dynamixel#2 Torque dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE) if dxl_comm_result != COMM_SUCCESS: print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: print("%s" % packetHandler.getRxPacketError(dxl_error)) else: print("Dynamixel#%d has been successfully connected" % DXL2_ID) # Enable Dynamixel#3 Torque dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL3_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE) if dxl_comm_result != COMM_SUCCESS: print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: print("%s" % packetHandler.getRxPacketError(dxl_error)) else: print("Dynamixel#%d has been successfully connected" % DXL3_ID) #PLAY NOTES index = 0 dxl2_goal_position = config["sensors"][0]["q2"] # Goal position 2 while index < len(notes_to_play): if notes_to_play[index]=='end': #end the loop if after last note break time.sleep(note_delay) note_delay = init_note_delay packetHandler.write4ByteTxRx(portHandler, 2, ADDR_PRO_GOAL_POSITION, dxl2_goal_position-80) #move up between notes to avoid collisions note_to_play = notes_to_play[index] #check the current note if note_to_play == 'half' or note_to_play == 'eigth': #skip the position if it is a 'half' or an 'eigth' index = index+1 note_to_play = notes_to_play[index] if notes_to_play[index+1] == 'half': #if the next position is /'half' wait longer/ after playing the current note note_delay = init_note_delay*4 elif notes_to_play[index+1] == 'eigth': #if the next position is /'eigth' wait less/ after playing the current note note_delay = init_note_delay/4 #Get note joint values from Json file dxl1_goal_position = config["sensors"][note_to_play-1]["q1"] # Goal position 1 dxl2_goal_position = config["sensors"][note_to_play-1]["q2"] # Goal position 2 dxl3_goal_position = config["sensors"][note_to_play-1]["q3"] # Goal position 3 while 1: #print("Press any key to continue! (or press ESC to quit!)") # #if getch() == chr(0x1b): #causes errors # break # # Move motor 1, then 3, and then 2 to avoid collisions # Motor 1 # Write goal position 1 dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL1_ID, ADDR_PRO_GOAL_POSITION, dxl1_goal_position) if dxl_comm_result != COMM_SUCCESS: print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: print("%s" % packetHandler.getRxPacketError(dxl_error)) time.sleep(delay) # Motor 3 # Write goal position 3 dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL3_ID, ADDR_PRO_GOAL_POSITION, dxl3_goal_position) if dxl_comm_result != COMM_SUCCESS: print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: print("%s" % packetHandler.getRxPacketError(dxl_error)) time.sleep(delay) # Motor 2 # Write goal position 2 dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL2_ID, ADDR_PRO_GOAL_POSITION, dxl2_goal_position) if dxl_comm_result != COMM_SUCCESS: print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: print("%s" % packetHandler.getRxPacketError(dxl_error)) time.sleep(delay) index = index + 1 break # go to next position #GO TO MIDDLE POSITION dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL2_ID, ADDR_PRO_GOAL_POSITION, 30) time.sleep(0.5) dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL3_ID, ADDR_PRO_GOAL_POSITION, 500) time.sleep(0.5) dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL1_ID, ADDR_PRO_GOAL_POSITION, 500) time.sleep(0.5) # DISABLE DYNAMIXEL TORQUES 1 2 3 dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE) if dxl_comm_result != COMM_SUCCESS: print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: print("%s" % packetHandler.getRxPacketError(dxl_error)) dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE) if dxl_comm_result != COMM_SUCCESS: print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: print("%s" % packetHandler.getRxPacketError(dxl_error)) dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL3_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE) if dxl_comm_result != COMM_SUCCESS: print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: print("%s" % packetHandler.getRxPacketError(dxl_error)) # Close port portHandler.closePort()