Repository navigation
Bus servo management. Improvements to framework including temperature management #143
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. Weβll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Merged
Merged
Changes from 1 commit
Commits
Show all changes
28 commits
Select commit
Hold shift + click to select a range
4c8c7d9
WIP bus servo management
danic85 d18189c
Update config
danic85 60d524a
Added SC servo examples + compatibility
danic85 3e4dfc0
Configure initial positions
danic85 0be65a3
Allow other poses to be stored and accessed
danic85 91fac95
Added change_id script
danic85 69f2d7c
Calibrate servos on boot
danic85 9a57f8a
adjustments
danic85 0cf906e
Dynamic servo calibration + center on change_id
danic85 8d62b54
Controller support and WIP recording
danic85 f02d0db
Working with Pi
danic85 b052ed0
Adjust config
danic85 6841f63
Cleanup
danic85 c1ed384
Working input recorder
danic85 5e503e5
Support servo control (WIP - calibration needed)
danic85 008c369
Callibration + allow modules to be disabled on test environments
danic85 af0fdb2
Disable test
danic85 40040c8
Add documentation and tests
danic85 f645caa
Fix test
danic85 8a22781
Timeout controller connection after 30 seconds
danic85 d81c07c
Enable on robot
danic85 6a84d43
Fix for environment input to resolve pi issue
danic85 4803d58
Calibrate new leg position
danic85 dc4cb27
Improvements to gamepad controller modules. Temperature alert in dispβ¦
danic85 d48ccd8
Fix tests
danic85 4a5a0ee
Fixes and improvements to loop and temperature management
danic85 5349956
Fix tests
danic85 cd4331b
Resolve PR comments
danic85 File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Added SC servo examples + compatibility
- Loading branch information
commit 60d524a92135493640e0921bc821b7fc73d763b0
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,78 @@ | ||
| #!/usr/bin/env python | ||
| # | ||
| # ********* Ping Example ********* | ||
| # | ||
| # | ||
| # Available SCServo model on this example : All models using Protocol SCS | ||
| # This example is tested with a SCServo(STS/SMS/SCS), and an URT | ||
| # Be sure that SCServo(STS/SMS/SCS) properties are already set as %% ID : 1 / Baudnum : 6 (Baudrate : 1000000) | ||
| # | ||
|
|
||
| import os | ||
|
|
||
| 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 scservo_sdk import * # Uses SCServo SDK library | ||
|
|
||
| # Default setting | ||
| SCS_ID = 1 # SCServo ID : 1 | ||
| BAUDRATE = 115200 # Driver board default baudrate : 115200 | ||
| DEVICENAME = '/dev/ttyUSB0' # Check which port is being used on your controller | ||
| # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" | ||
|
|
||
| protocol_end = 1 # SCServo bit end(STS/SMS=0, SCS=1) | ||
|
|
||
| # Initialize PortHandler instance | ||
| # Set the port path | ||
| # Get methods and members of PortHandlerLinux or PortHandlerWindows | ||
| portHandler = PortHandler(DEVICENAME) | ||
|
|
||
| # Initialize PacketHandler instance | ||
| # Get methods and members of Protocol | ||
| packetHandler = PacketHandler(protocol_end) | ||
|
|
||
| # 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() | ||
|
|
||
| # Try to ping the SCServo | ||
| # Get SCServo model number | ||
| scs_model_number, scs_comm_result, scs_error = packetHandler.ping(portHandler, SCS_ID) | ||
| if scs_comm_result != COMM_SUCCESS: | ||
| print("%s" % packetHandler.getTxRxResult(scs_comm_result)) | ||
| elif scs_error != 0: | ||
| print("%s" % packetHandler.getRxPacketError(scs_error)) | ||
| else: | ||
| print("[ID:%03d] ping Succeeded. SCServo model number : %d" % (SCS_ID, scs_model_number)) | ||
|
|
||
| # Close port | ||
| portHandler.closePort() | ||
138 changes: 138 additions & 0 deletions
138
modules/actuators/bus_servo/SCServo_examples/read_write.py
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,138 @@ | ||
| #!/usr/bin/env python | ||
| # | ||
| # ********* Gen Write Example ********* | ||
| # | ||
| # | ||
| # Available SCServo model on this example : All models using Protocol SCS | ||
| # This example is tested with a SCServo(STS/SMS/SCS), and an URT | ||
| # Be sure that SCServo(STS/SMS/SCS) properties are already set as %% ID : 1 / Baudnum : 6 (Baudrate : 1000000) | ||
| # | ||
|
|
||
| import os | ||
|
|
||
| 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 scservo_sdk import * # Uses SCServo SDK library | ||
|
danic85 marked this conversation as resolved.
Resolved
|
||
|
|
||
| # Control table address | ||
| ADDR_SCS_TORQUE_ENABLE = 40 | ||
| ADDR_SCS_GOAL_ACC = 41 | ||
| ADDR_SCS_GOAL_POSITION = 42 | ||
| ADDR_SCS_GOAL_SPEED = 46 | ||
| ADDR_SCS_PRESENT_POSITION = 56 | ||
|
|
||
| # Default setting | ||
| SCS_ID = 1 # SCServo ID : 1 | ||
| BAUDRATE = 115200 # Driver board default baudrate : 115200 | ||
| DEVICENAME = '/dev/ttyUSB0' # Check which port is being used on your controller | ||
| # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" | ||
|
|
||
| SCS_MINIMUM_POSITION_VALUE = 100 # SCServo will rotate between this value | ||
| SCS_MAXIMUM_POSITION_VALUE = 4000 # and this value (note that the SCServo would not move when the position value is out of movable range. Check e-manual about the range of the SCServo you use.) | ||
| SCS_MOVING_STATUS_THRESHOLD = 20 # SCServo moving status threshold | ||
| SCS_MOVING_SPEED = 0 # SCServo moving speed | ||
| SCS_MOVING_ACC = 0 # SCServo moving acc | ||
| protocol_end = 1 # SCServo bit end(STS/SMS=0, SCS=1) | ||
|
|
||
| index = 0 | ||
| scs_goal_position = [SCS_MINIMUM_POSITION_VALUE, SCS_MAXIMUM_POSITION_VALUE] # Goal position | ||
|
|
||
|
|
||
| # Initialize PortHandler instance | ||
| # Set the port path | ||
| # Get methods and members of PortHandlerLinux or PortHandlerWindows | ||
| portHandler = PortHandler(DEVICENAME) | ||
|
|
||
| # Initialize PacketHandler instance | ||
| # Get methods and members of Protocol | ||
| packetHandler = PacketHandler(protocol_end) | ||
|
|
||
| # 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() | ||
|
|
||
| # Write SCServo acc | ||
| scs_comm_result, scs_error = packetHandler.write1ByteTxRx(portHandler, SCS_ID, ADDR_SCS_GOAL_ACC, SCS_MOVING_ACC) | ||
| if scs_comm_result != COMM_SUCCESS: | ||
| print("%s" % packetHandler.getTxRxResult(scs_comm_result)) | ||
| elif scs_error != 0: | ||
| print("%s" % packetHandler.getRxPacketError(scs_error)) | ||
|
|
||
| # Write SCServo speed | ||
| scs_comm_result, scs_error = packetHandler.write2ByteTxRx(portHandler, SCS_ID, ADDR_SCS_GOAL_SPEED, SCS_MOVING_SPEED) | ||
| if scs_comm_result != COMM_SUCCESS: | ||
| print("%s" % packetHandler.getTxRxResult(scs_comm_result)) | ||
| elif scs_error != 0: | ||
| print("%s" % packetHandler.getRxPacketError(scs_error)) | ||
|
|
||
| while 1: | ||
| print("Press any key to continue! (or press ESC to quit!)") | ||
| if getch() == chr(0x1b): | ||
| break | ||
|
|
||
| # Write SCServo goal position | ||
| scs_comm_result, scs_error = packetHandler.write2ByteTxRx(portHandler, SCS_ID, ADDR_SCS_GOAL_POSITION, scs_goal_position[index]) | ||
| if scs_comm_result != COMM_SUCCESS: | ||
| print("%s" % packetHandler.getTxRxResult(scs_comm_result)) | ||
| elif scs_error != 0: | ||
| print("%s" % packetHandler.getRxPacketError(scs_error)) | ||
|
|
||
| while 1: | ||
| # Read SCServo present position | ||
| scs_present_position_speed, scs_comm_result, scs_error = packetHandler.read4ByteTxRx(portHandler, SCS_ID, ADDR_SCS_PRESENT_POSITION) | ||
| if scs_comm_result != COMM_SUCCESS: | ||
| print(packetHandler.getTxRxResult(scs_comm_result)) | ||
| elif scs_error != 0: | ||
| print(packetHandler.getRxPacketError(scs_error)) | ||
|
|
||
| scs_present_position = SCS_LOWORD(scs_present_position_speed) | ||
| scs_present_speed = SCS_HIWORD(scs_present_position_speed) | ||
| print("[ID:%03d] GoalPos:%03d PresPos:%03d PresSpd:%03d" | ||
| % (SCS_ID, scs_goal_position[index], scs_present_position, SCS_TOHOST(scs_present_speed, 15))) | ||
|
|
||
| if not (abs(scs_goal_position[index] - scs_present_position_speed) > SCS_MOVING_STATUS_THRESHOLD): | ||
| break | ||
|
|
||
|
|
||
| # Change goal position | ||
| if index == 0: | ||
| index = 1 | ||
| else: | ||
| index = 0 | ||
|
|
||
| scs_comm_result, scs_error = packetHandler.write1ByteTxRx(portHandler, SCS_ID, ADDR_SCS_TORQUE_ENABLE, 0) | ||
| if scs_comm_result != COMM_SUCCESS: | ||
| print("%s" % packetHandler.getTxRxResult(scs_comm_result)) | ||
| elif scs_error != 0: | ||
| print("%s" % packetHandler.getRxPacketError(scs_error)) | ||
| # Close port | ||
| portHandler.closePort() | ||
6 changes: 6 additions & 0 deletions
6
modules/actuators/bus_servo/SCServo_examples/scservo_sdk/__init__.py
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,6 @@ | ||
| #!/usr/bin/env python | ||
|
|
||
| from .port_handler import * | ||
| from .packet_handler import * | ||
| from .group_sync_read import * | ||
|
danic85 marked this conversation as resolved.
Resolved
|
||
| from .group_sync_write import * | ||
|
danic85 marked this conversation as resolved.
Resolved
danic85 marked this conversation as resolved.
Resolved
danic85 marked this conversation as resolved.
Resolved
|
||
110 changes: 110 additions & 0 deletions
110
modules/actuators/bus_servo/SCServo_examples/scservo_sdk/group_sync_read.py
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,110 @@ | ||
| #!/usr/bin/env python | ||
|
|
||
| from .scservo_def import * | ||
|
danic85 marked this conversation as resolved.
Resolved
|
||
|
|
||
| class GroupSyncRead: | ||
| def __init__(self, port, ph, start_address, data_length): | ||
| self.port = port | ||
| self.ph = ph | ||
| self.start_address = start_address | ||
| self.data_length = data_length | ||
|
|
||
| self.last_result = False | ||
| self.is_param_changed = False | ||
| self.param = [] | ||
| self.data_dict = {} | ||
|
|
||
| self.clearParam() | ||
|
|
||
| def makeParam(self): | ||
| if not self.data_dict: # len(self.data_dict.keys()) == 0: | ||
| return | ||
|
|
||
| self.param = [] | ||
|
|
||
| for scs_id in self.data_dict: | ||
| self.param.append(scs_id) | ||
|
|
||
| def addParam(self, scs_id): | ||
| if scs_id in self.data_dict: # scs_id already exist | ||
| return False | ||
|
|
||
| self.data_dict[scs_id] = [] # [0] * self.data_length | ||
|
|
||
| self.is_param_changed = True | ||
| return True | ||
|
|
||
| def removeParam(self, scs_id): | ||
| if scs_id not in self.data_dict: # NOT exist | ||
| return | ||
|
|
||
| del self.data_dict[scs_id] | ||
|
|
||
| self.is_param_changed = True | ||
|
|
||
| def clearParam(self): | ||
| self.data_dict.clear() | ||
|
|
||
| def txPacket(self): | ||
| if len(self.data_dict.keys()) == 0: | ||
| return COMM_NOT_AVAILABLE | ||
|
|
||
| if self.is_param_changed is True or not self.param: | ||
| self.makeParam() | ||
|
|
||
| return self.ph.syncReadTx(self.port, self.start_address, self.data_length, self.param, | ||
| len(self.data_dict.keys()) * 1) | ||
|
|
||
| def rxPacket(self): | ||
| self.last_result = False | ||
|
|
||
| result = COMM_RX_FAIL | ||
|
|
||
| if len(self.data_dict.keys()) == 0: | ||
| return COMM_NOT_AVAILABLE | ||
|
|
||
| for scs_id in self.data_dict: | ||
| self.data_dict[scs_id], result, _ = self.ph.readRx(self.port, scs_id, self.data_length) | ||
| if result != COMM_SUCCESS: | ||
| return result | ||
|
|
||
| if result == COMM_SUCCESS: | ||
| self.last_result = True | ||
|
|
||
| return result | ||
|
|
||
| def txRxPacket(self): | ||
| result = self.txPacket() | ||
| if result != COMM_SUCCESS: | ||
| return result | ||
|
|
||
| return self.rxPacket() | ||
|
|
||
| def isAvailable(self, scs_id, address, data_length): | ||
| #if self.last_result is False or scs_id not in self.data_dict: | ||
| if scs_id not in self.data_dict: | ||
| return False | ||
|
|
||
| if (address < self.start_address) or (self.start_address + self.data_length - data_length < address): | ||
| return False | ||
|
|
||
| if len(self.data_dict[scs_id])<data_length: | ||
| return False | ||
| return True | ||
|
|
||
| def getData(self, scs_id, address, data_length): | ||
| if not self.isAvailable(scs_id, address, data_length): | ||
| return 0 | ||
|
|
||
| if data_length == 1: | ||
| return self.data_dict[scs_id][address - self.start_address] | ||
| elif data_length == 2: | ||
| return SCS_MAKEWORD(self.data_dict[scs_id][address - self.start_address], | ||
| self.data_dict[scs_id][address - self.start_address + 1]) | ||
| elif data_length == 4: | ||
| return SCS_MAKEDWORD(SCS_MAKEWORD(self.data_dict[scs_id][address - self.start_address + 0], | ||
| self.data_dict[scs_id][address - self.start_address + 1]), | ||
| SCS_MAKEWORD(self.data_dict[scs_id][address - self.start_address + 2], | ||
| self.data_dict[scs_id][address - self.start_address + 3])) | ||
| else: | ||
| return 0 | ||
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Uh oh!
There was an error while loading. Please reload this page.