From 0a2a92b35608f747d79ce75ef6c7d2c36e4bdcc9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rapha=C3=ABl=20SAVIO?= Date: Thu, 24 Apr 2025 11:03:40 +0200 Subject: [PATCH] Uploaded Robot Controller python program and corresponding json file --- pysrc/robotController_V2.py | 254 ++++++++++++++++++++++++++++++++++++ pysrc/sensor_positions.json | 100 ++++++++++++++ 2 files changed, 354 insertions(+) create mode 100644 pysrc/robotController_V2.py create mode 100644 pysrc/sensor_positions.json diff --git a/pysrc/robotController_V2.py b/pysrc/robotController_V2.py new file mode 100644 index 0000000..75c06ac --- /dev/null +++ b/pysrc/robotController_V2.py @@ -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() diff --git a/pysrc/sensor_positions.json b/pysrc/sensor_positions.json new file mode 100644 index 0000000..30330c7 --- /dev/null +++ b/pysrc/sensor_positions.json @@ -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 + } + ] +} \ No newline at end of file