guillaume.bonabau
/
Robotic_Piano_Player
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Uploaded Robot Controller python program and corresponding json file

This commit is contained in:
Raphaël SAVIO 2025-04-24 11:03:40 +02:00
parent 9ff4e22796
commit 0a2a92b356
2 changed files with 354 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

254
pysrc/robotController_V2.py Normal file
View File

@ -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()

100
pysrc/sensor_positions.json Normal file
View File

@ -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
}
]
}