Merge branch 'main' of https://gitarero.ecam.fr/guillaume.bonabau/Robotic_Piano_Player
This commit is contained in:
commit
201c3cafc7
|
|
@ -0,0 +1,254 @@
|
|||
#!/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()
|
||||
|
|
@ -0,0 +1,100 @@
|
|||
{
|
||||
"sensors": [
|
||||
{
|
||||
"id": 1,
|
||||
"note": "C",
|
||||
"type": "natural",
|
||||
"q1": 770,
|
||||
"q2": 170,
|
||||
"q3": 530
|
||||
},
|
||||
{
|
||||
"id": 2,
|
||||
"note": "C",
|
||||
"type": "sharp",
|
||||
"q1": 800,
|
||||
"q2": 89,
|
||||
"q3": 735
|
||||
},
|
||||
{
|
||||
"id": 3,
|
||||
"note": "D",
|
||||
"type": "natural",
|
||||
"q1": 715,
|
||||
"q2": 170,
|
||||
"q3": 530
|
||||
},
|
||||
{
|
||||
"id": 4,
|
||||
"note": "D",
|
||||
"type": "sharp",
|
||||
"q1": 650,
|
||||
"q2": 89,
|
||||
"q3": 735
|
||||
},
|
||||
{
|
||||
"id": 5,
|
||||
"note": "E",
|
||||
"type": "natural",
|
||||
"q1": 600 ,
|
||||
"q2": 170,
|
||||
"q3": 530
|
||||
},
|
||||
{
|
||||
"id": 6,
|
||||
"note": "F",
|
||||
"type": "natural",
|
||||
"q1": 520,
|
||||
"q2": 170,
|
||||
"q3": 530
|
||||
},
|
||||
{
|
||||
"id": 7,
|
||||
"note": "F",
|
||||
"type": "sharp",
|
||||
"q1": 516,
|
||||
"q2": 89,
|
||||
"q3": 735
|
||||
},
|
||||
{
|
||||
"id": 8,
|
||||
"note": "G",
|
||||
"type": "natural",
|
||||
"q1": 445,
|
||||
"q2": 170,
|
||||
"q3": 530
|
||||
},
|
||||
{
|
||||
"id": 9,
|
||||
"note": "G",
|
||||
"type": "sharp",
|
||||
"q1": 360,
|
||||
"q2": 89,
|
||||
"q3": 735
|
||||
},
|
||||
{
|
||||
"id": 10,
|
||||
"note": "A",
|
||||
"type": "natural",
|
||||
"q1": 333,
|
||||
"q2": 170,
|
||||
"q3": 530
|
||||
},
|
||||
{
|
||||
"id": 11,
|
||||
"note": "A",
|
||||
"type": "sharp",
|
||||
"q1": 229,
|
||||
"q2": 89,
|
||||
"q3": 735
|
||||
},
|
||||
{
|
||||
"id": 12,
|
||||
"note": "B",
|
||||
"type": "natural",
|
||||
"q1": 270,
|
||||
"q2": 170,
|
||||
"q3": 530
|
||||
}
|
||||
]
|
||||
}
|
||||
Loading…
Reference in New Issue