Skip to content

Commit

Permalink
Modified
Browse files Browse the repository at this point in the history
  • Loading branch information
@iory
iory committed Nov 12, 2024
1 parent e0cb5df commit ef79ccf
Show file tree
Hide file tree
Showing 3 changed files with 107 additions and 87 deletions.
8 changes: 8 additions & 0 deletions README.md
View file
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
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
184 changes: 98 additions & 86 deletions rcb4/ics.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -141,7 +141,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 +277,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 Down Expand Up @@ -323,94 +325,99 @@ 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:
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:
self.close_connection()
continue
# Flush the output to ensure it displays correctly
sys.stdout.flush()

Expand Down Expand Up @@ -511,3 +518,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()

0 comments on commit ef79ccf

Please sign in to comment.