From 8f18858e576225d0f55f61ea5925c061d4af987e Mon Sep 17 00:00:00 2001 From: JX Date: Mon, 2 Dec 2024 15:37:55 -0800 Subject: [PATCH] refactor --- docs | 2 +- experiments/calibrate_all.py | 111 ------------------------ experiments/keyboard_control.py | 144 -------------------------------- sim | 2 +- 4 files changed, 2 insertions(+), 257 deletions(-) delete mode 100644 experiments/calibrate_all.py delete mode 100644 experiments/keyboard_control.py diff --git a/docs b/docs index 8198cca..c99e324 160000 --- a/docs +++ b/docs @@ -1 +1 @@ -Subproject commit 8198cca4935551f8afcce52af60d1af3eb94d450 +Subproject commit c99e3241c7af58d718dc06d56879007bff5cc643 diff --git a/experiments/calibrate_all.py b/experiments/calibrate_all.py deleted file mode 100644 index 542d95b..0000000 --- a/experiments/calibrate_all.py +++ /dev/null @@ -1,111 +0,0 @@ -from openlch import HAL -import time -import sys - - -def get_servo_positions(hal: HAL) -> list: - servo_positions = hal.servo.get_positions() - positions = [pos for _, pos in servo_positions[:10]] - print(f"[INFO]: GET servo positions: {positions}") - return positions - - -def set_servo_positions(positions: list, hal: HAL) -> None: - print(f"[INFO]: SET servo positions: {positions}") - servo_positions = [(i, pos) for i, pos in enumerate(positions[:10])] - hal.servo.set_positions(servo_positions) - - -""" -TODO: -- incrase torque for calibration -- add specific positions for differnet servos when calibrating (sequence) -""" - - -def set_servo_position(servo_id: int, position: float, hal: HAL) -> None: - print(f"[INFO]: Setting servo {servo_id} to position {position}") - hal.servo.set_positions([(servo_id - 1, position)]) - time.sleep(0.5) - - -def calibrate_all_servos(hal: HAL) -> None: - # Calibration sequence with specific positions - sequence = [ - (13, 1.0), # max position - (14, 1.0), # max position - (12, 0.0), - (15, 0.0), - (11, 0.0), - (16, 0.0), - (4, 1.0), # max position - (9, 1.0), # max position - (5, 0.0), - (10, 0.0), - (3, 0.0), - (8, 0.0), - (2, 0.0), - (7, 0.0), - (1, 0.0), - (6, 0.0), - ] - - try: - for servo_id, position in sequence: - print(f"[INFO]: Started servo {servo_id} calibration =====") - - while True: - status = hal.servo.get_calibration_status() - if not status.get("is_calibrating", False): - break - time.sleep(1) - - hal.servo.start_calibration(servo_id) - - except Exception as e: - print(f"[ERROR]: Calibration failed: {str(e)}") - cancel_all_calibrations(hal) - raise e - - -def cancel_all_calibrations(hal: HAL) -> None: - print("[INFO]: Canceling all previous calibrations...") - for i in range(1, 16): - try: - hal.servo.cancel_calibration(i) - time.sleep(0.1) - except Exception as e: - if "No calibration is currently in progress" in str(e): - continue - raise e - - -if __name__ == "__main__": - hal = HAL() - - try: - print(hal.servo.scan()) - - cancel_all_calibrations(hal) - - print("[INFO]: Setting torque...") - hal.servo.set_torque_enable([(i, True) for i in range(1, 17)]) - time.sleep(1) - hal.servo.set_torque([(i, 20.0) for i in range(1, 17)]) - time.sleep(1) - - calibrate_all_servos(hal) - - print("[INFO]: Setting positions to 0...") - time.sleep(1) - - hal.servo.set_positions([(i, 0.0) for i in range(1, 17)]) - - except KeyboardInterrupt: - print("\n[INFO]: Keyboard interrupt detected. Canceling calibrations...") - cancel_all_calibrations(hal) - sys.exit(0) - except Exception as e: - print(f"[ERROR]: An error occurred: {str(e)}") - cancel_all_calibrations(hal) - sys.exit(1) diff --git a/experiments/keyboard_control.py b/experiments/keyboard_control.py deleted file mode 100644 index fedb6e2..0000000 --- a/experiments/keyboard_control.py +++ /dev/null @@ -1,144 +0,0 @@ -import pygame -import time -import datetime -import csv -import os -from robot import Robot, RobotConfig - -joint_positions = {} - -if not os.path.exists("logs"): - os.makedirs("logs") - -log_filename = os.path.join( - "logs", f"servo_movements_{datetime.datetime.now().strftime('%Y%m%d_%H%M%S')}.csv" -) -csv_file = open(log_filename, "w", newline="") -csv_writer = csv.writer(csv_file) -csv_writer.writerow( - ["Timestamp", "Joint Name", "Servo ID", "Angle (degrees)"] -) - - -def main(): - global joint_positions # Indicate that we're using the global variable - - robot = Robot() - robot.initialize() - - servo_id_to_joint = {joint.servo_id: joint for joint in robot.joints} - - def prompt_for_joint(): - while True: - user_input = input("Enter the servo ID you want to control: ").strip() - filtered_input = "".join(filter(str.isdigit, user_input)) - if not filtered_input: - print("Invalid input. Please enter a numeric servo ID.") - continue - try: - servo_id_input = int(filtered_input) - if servo_id_input not in servo_id_to_joint: - print(f"Servo ID {servo_id_input} not found.") - continue - return servo_id_to_joint[servo_id_input] - except ValueError: - print("Invalid input. Please enter a numeric servo ID.") - - pygame.init() - screen = pygame.display.set_mode((400, 300)) - pygame.display.set_caption("Servo Control") - - # Get initial feedback positions - joint_positions = robot.get_feedback_positions() - - joint = prompt_for_joint() - if not joint: - return - - print(f"Controlling joint '{joint.name}' with servo ID {joint.servo_id}") - - current_position = joint_positions.get(joint.name, 0.0) - print(f"Joint '{joint.name}' current angle is {current_position:.2f} degrees") - - running = True - while running: - time.sleep(0.01) - for event in pygame.event.get(): - if event.type == pygame.QUIT: - running = False - - elif event.type == pygame.KEYDOWN: - if event.key == pygame.K_ESCAPE: - running = False - - elif event.key == pygame.K_q: - # Before switching joints, update the position of the current joint - joint_positions[joint.name] = current_position - - new_joint = prompt_for_joint() - if new_joint: - joint = new_joint - # Retrieve the position for the new joint - current_position = joint_positions.get(joint.name, 0.0) - print( - f"Switched to controlling joint '{joint.name}' with servo ID {joint.servo_id}" - ) - print( - f"Joint '{joint.name}' current angle is {current_position:.2f} degrees" - ) - - elif event.key == pygame.K_UP: - # Increase joint angle by 10 degrees - current_position += 10.0 - robot.set_desired_positions({joint.name: current_position}) - joint_positions[joint.name] = current_position - print( - f"Joint '{joint.name}' angle increased to {current_position:.2f} degrees" - ) - # Log to CSV - csv_writer.writerow( - [ - datetime.datetime.now().strftime("%Y-%m-%d %H:%M:%S"), - joint.name, - joint.servo_id, - f"{current_position:.2f}", - ] - ) - csv_file.flush() # Ensure data is written immediately - - elif event.key == pygame.K_DOWN: - # Decrease joint angle by 10 degrees - current_position -= 10.0 - robot.set_desired_positions({joint.name: current_position}) - joint_positions[joint.name] = current_position - print( - f"Joint '{joint.name}' angle decreased to {current_position:.2f} degrees" - ) - # Log to CSV - csv_writer.writerow( - [ - datetime.datetime.now().strftime("%Y-%m-%d %H:%M:%S"), - joint.name, - joint.servo_id, - f"{current_position:.2f}", - ] - ) - csv_file.flush() # Ensure data is written immediately - - try: - pass - except KeyboardInterrupt: - print("\nCtrl+C detected, shutting down gracefully...") - running = False - - try: - robot.disable_motors() - print("Motors disabled") - csv_file.close() # Close the CSV file - except Exception as e: - print(f"Error disabling motors: {e}") - pygame.quit() - - -if __name__ == "__main__": - main() diff --git a/sim b/sim index 4827e5a..3843e4c 160000 --- a/sim +++ b/sim @@ -1 +1 @@ -Subproject commit 4827e5af3c0fc40dd6df5c58ec86f0ce4959dfbe +Subproject commit 3843e4cd25af9c160a33f86fb9a41ab17dd732b7