Skip to content
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.

Sign up for GitHub

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

Bump to 0.0.4 #117

Merged
merged 2 commits into from
Nov 12, 2024
Merged

Bump to 0.0.4 #117

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
63ef0fd
Modified
iory Nov 12, 2024
3d608c5
0.0.4
iory Nov 12, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 8 additions & 0 deletions README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,14 @@ Connect st-link and run the following command.
rcb4-write-firmware
```

## Change Servo ID

You can use ics-manager command to change servo id.

```bash
ics-manager
```

## Contributing

### Automatic Formatting
Expand Down
2 changes: 1 addition & 1 deletion rcb4/apps/ics_manager.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@ def parse_args():
"""Parse command-line arguments."""
parser = argparse.ArgumentParser(description="ICS Servo Controller CLI Tool")
parser.add_argument(
"--yaml_path",
"--yaml-path",
default=None,
help="Path to YAML configuration file for servo settings"
)
Expand Down
194 changes: 108 additions & 86 deletions rcb4/ics.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -91,6 +91,9 @@ def __init__(self, baudrate=1250000, yaml_path=None):

def open_connection(self):
ports = serial.tools.list_ports.comports()
if len(ports) == 0:
print(f"{Fore.RED}No USB Found.{Style.RESET_ALL}")
print(f"{Fore.RED}May Dual USB Adapter is wrong.{Style.RESET_ALL}")
for p in ports:
if p.vid == 0x165C and p.pid == 0x08:
for baudrate in [115200, 625000, 1250000]:
Expand Down Expand Up @@ -141,7 +144,8 @@ def get_servo_id(self):
self.ics.write(bytes([0xFF, 0x00, 0x00, 0x00]))
time.sleep(0.1)
ret = self.ics.read(5)
return ret[4] & 0x1F
servo_id = ret[4] & 0x1F
return servo_id

def set_servo_id(self, servo_id):
self.ics.write(bytes([0xE0 | (0x1F & servo_id), 0x01, 0x01, 0x01]))
Expand Down Expand Up @@ -276,6 +280,7 @@ def set_param(self, ics_param64, servo_id=None):
def close_connection(self):
if self.ics and self.ics.is_open:
self.ics.close()
self.ics = None

def display_status(self):
options = [
Expand All @@ -291,6 +296,7 @@ def display_status(self):
selectable_options = ["Current Servo ID", "Angle"]
try:
use_previous_result = False
previous_servo_id = None
while True:
if not self.ics or not self.ics.is_open:
# Clear the previous output at the start of each loop
Expand Down Expand Up @@ -323,94 +329,105 @@ def display_status(self):
sys.stdout.flush()

# Print servo status
print("--- Servo Status ---")
if use_previous_result is False:
_, result = self.read_param()
for i, option in enumerate(options):
if i == self.selected_index:
print(
f">> {option}: {self.get_status(option, result, selected=True)}"
)
else:
print(f" {option}: {self.get_status(option, result, selected=False)}")
print("----------------------\n")
print(
"Use ↑↓ to navigate, ←→ to adjust Current Servo ID or Servo Angles"
)
print(f"Press 'Enter' when Current Servo ID is selected to save the currently highlighted ID in {Fore.GREEN}green{Style.RESET_ALL}.")
print("Press 'z' to reset servo position")
print(
"Press 'r' to toggle rotation mode (enables continuous wheel-like rotation)"
)
print("Press 'f' to set free mode\n")
print("'q' to quit.")

use_previous_result = False

# Get key input without Enter
key = readchar.readkey()

# Perform actions based on key
if key == "z":
self.reset_servo_position()
elif key == "r":
self.toggle_rotation_mode()
elif key == "f":
self.set_free_mode()
elif key == "q":
print("Exiting...")
break
elif key == readchar.key.UP:
self.selected_index = (self.selected_index - 1) % len(
selectable_options
try:
servo_id = self.get_servo_id()
if servo_id != previous_servo_id:
use_previous_result = False
previous_servo_id = servo_id
print("--- Servo Status ---")
if use_previous_result is False:
_, result = self.read_param()
for i, option in enumerate(options):
if i == self.selected_index:
print(
f">> {option}: {self.get_status(option, result, selected=True)}"
)
else:
print(f" {option}: {self.get_status(option, result, selected=False)}")

print("----------------------\n")
print(
"Use ↑↓ to navigate, ←→ to adjust Current Servo ID or Servo Angles"
)
use_previous_result = True
elif key == readchar.key.DOWN:
self.selected_index = (self.selected_index + 1) % len(
selectable_options
print(f"Press 'Enter' when Current Servo ID is selected to save the currently highlighted ID in {Fore.GREEN}green{Style.RESET_ALL}.")
print("Press 'z' to reset servo position")
print(
"Press 'r' to toggle rotation mode (enables continuous wheel-like rotation)"
)
use_previous_result = True
elif (
key == readchar.key.ENTER
and selectable_options[self.selected_index] == "Current Servo ID"
):
self.set_servo_id(self.servo_id)
time.sleep(0.3)
if self.servo_id_to_rotation is not None:
self.set_rotation(self.servo_id_to_rotation[self.servo_id])
time.sleep(0.3)
elif (
key == readchar.key.LEFT
and selectable_options[self.selected_index] == "Current Servo ID"
):
use_previous_result = True
if self.servo_id_index == 0:
self.servo_id_index = len(self.servo_candidates) - 1
else:
self.servo_id_index = max(0, self.servo_id_index - 1)
self.servo_id = self.servo_candidates[self.servo_id_index]
elif (
key == readchar.key.RIGHT
and selectable_options[self.selected_index] == "Current Servo ID"
):
use_previous_result = True
if self.servo_id_index == len(self.servo_candidates) - 1:
self.servo_id_index = 0
else:
self.servo_id_index = min(
len(self.servo_candidates) - 1, self.servo_id_index + 1
print("Press 'f' to set free mode\n")
print("'q' to quit.")

use_previous_result = False

# Get key input without Enter
key = readchar.readkey()

# Perform actions based on key
if key == "z":
self.reset_servo_position()
elif key == "r":
self.toggle_rotation_mode()
elif key == "f":
self.set_free_mode()
elif key == "q":
print("Exiting...")
break
elif key == readchar.key.UP:
self.selected_index = (self.selected_index - 1) % len(
selectable_options
)
use_previous_result = True
elif key == readchar.key.DOWN:
self.selected_index = (self.selected_index + 1) % len(
selectable_options
)
self.servo_id = self.servo_candidates[self.servo_id_index]
elif (
key == readchar.key.LEFT
and selectable_options[self.selected_index] == "Angle"
):
self.decrease_angle()
elif (
key == readchar.key.RIGHT
and selectable_options[self.selected_index] == "Angle"
):
self.increase_angle()
use_previous_result = True
elif (
key == readchar.key.ENTER
and selectable_options[self.selected_index] == "Current Servo ID"
):
self.set_servo_id(self.servo_id)
time.sleep(0.3)
if self.servo_id_to_rotation is not None:
self.set_rotation(self.servo_id_to_rotation[self.servo_id])
time.sleep(0.3)
elif (
key == readchar.key.LEFT
and selectable_options[self.selected_index] == "Current Servo ID"
):
use_previous_result = True
if self.servo_id_index == 0:
self.servo_id_index = len(self.servo_candidates) - 1
else:
self.servo_id_index = max(0, self.servo_id_index - 1)
self.servo_id = self.servo_candidates[self.servo_id_index]
elif (
key == readchar.key.RIGHT
and selectable_options[self.selected_index] == "Current Servo ID"
):
use_previous_result = True
if self.servo_id_index == len(self.servo_candidates) - 1:
self.servo_id_index = 0
else:
self.servo_id_index = min(
len(self.servo_candidates) - 1, self.servo_id_index + 1
)
self.servo_id = self.servo_candidates[self.servo_id_index]
elif (
key == readchar.key.LEFT
and selectable_options[self.selected_index] == "Angle"
):
self.decrease_angle()
elif (
key == readchar.key.RIGHT
and selectable_options[self.selected_index] == "Angle"
):
self.increase_angle()
except Exception as e:
print(f'[ERROR] {e}')
use_previous_result = False
self.close_connection()
continue
# Flush the output to ensure it displays correctly
sys.stdout.flush()

Expand Down Expand Up @@ -511,3 +528,8 @@ def _4bit2num(self, lst, v):
for i in lst:
sum_val = (sum_val << 4) + (v[i - 1] & 0x0F)
return sum_val


if __name__ == '__main__':
servo_controller = ICSServoController()
servo_controller.open_connection()
2 changes: 1 addition & 1 deletion ros/kxr_controller/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>kxr_controller</name>
<version>0.0.3</version>
<version>0.0.4</version>
<description>The kxr_controller package</description>

<maintainer email="[email protected]">Iori Yanokura</maintainer>
Expand Down
2 changes: 1 addition & 1 deletion ros/kxr_models/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
<package format="2">
<name>kxr_models</name>
<version>0.0.3</version>
<version>0.0.4</version>
<description>
<p>URDF Description package for kxr_models</p>
<p>This package contains configuration data, 3D models and launch files for kxr robot</p>
Expand Down
2 changes: 1 addition & 1 deletion ros/kxreus/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
<package format="3">

<name>kxreus</name>
<version>0.0.3</version>
<version>0.0.4</version>
<description>
roseus interface for kxr robots.
</description>
Expand Down
2 changes: 1 addition & 1 deletion setup.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
from setuptools import find_packages
from setuptools import setup

version = "0.0.3"
version = "0.0.4"


if sys.argv[-1] == "release":
Expand Down
Loading