diff --git a/examples.zip b/examples.zip new file mode 100644 index 00000000..788c8873 Binary files /dev/null and b/examples.zip differ diff --git a/examples/hopejr/exoskeleton/plottest7.py b/examples/hopejr/exoskeleton/plottest7.py new file mode 100644 index 00000000..14da79d9 --- /dev/null +++ b/examples/hopejr/exoskeleton/plottest7.py @@ -0,0 +1,64 @@ +import serial +import matplotlib.pyplot as plt +import matplotlib.animation as animation +from collections import deque + +# Config +SERIAL_PORT = '/dev/ttyACM1' # Change as needed +BAUD_RATE = 115200 +BUFFER_LEN = 200 + +# Sensor names in order +sensor_names = [ + "wrist_roll", + "wrist_pitch", + "wrist_yaw", + "elbow_flex", + "shoulder_roll", + "shoulder_yaw", + "shoulder_pitch" +] + +# Initialize buffers +sensor_data = { + name: deque([0]*BUFFER_LEN, maxlen=BUFFER_LEN) + for name in sensor_names +} + +# Setup plot +fig, axes = plt.subplots(len(sensor_names), 1, figsize=(8, 12), sharex=True) +fig.tight_layout(pad=3.0) + +lines = {} +for i, name in enumerate(sensor_names): + axes[i].set_title(name) + axes[i].set_xlim(0, BUFFER_LEN) + axes[i].set_ylim(0, 4096) + line, = axes[i].plot([], [], label=name) + axes[i].legend() + lines[name] = line + +# Connect to serial +ser = serial.Serial(SERIAL_PORT, BAUD_RATE) + +# Update function +def update(frame): + while ser.in_waiting: + line = ser.readline().decode().strip() + parts = line.split() + if len(parts) != 7: + continue + try: + values = list(map(int, parts)) + except ValueError: + continue + for i, name in enumerate(sensor_names): + sensor_data[name].append(values[i]) + for name in sensor_names: + x = range(len(sensor_data[name])) + lines[name].set_data(x, sensor_data[name]) + return lines.values() + +# Animate +ani = animation.FuncAnimation(fig, update, interval=50, blit=False) +plt.show() diff --git a/examples/hopejr/exoskeleton/plottestmulti.py b/examples/hopejr/exoskeleton/plottestmulti.py index 1b26bef7..f5ff7bec 100644 --- a/examples/hopejr/exoskeleton/plottestmulti.py +++ b/examples/hopejr/exoskeleton/plottestmulti.py @@ -13,8 +13,9 @@ ser = serial.Serial(SERIAL_PORT, BAUD_RATE) # How many data points to keep in the scrolling buffer buffer_len = 200 -# Create buffers for each sensor pair. -# We'll store them in a dict to keep things organized. +# ------------------------------------------------------------------- +# 1) Sensor buffers for existing sensors + new wrist_pitch, wrist_yaw +# ------------------------------------------------------------------- sensor_buffers = { 'wrist_roll': { 'val1': deque([0]*buffer_len, maxlen=buffer_len), @@ -35,39 +36,57 @@ sensor_buffers = { 'shoulder_roll': { 'val1': deque([0]*buffer_len, maxlen=buffer_len), 'val2': deque([0]*buffer_len, maxlen=buffer_len) - } + }, + # --- New single-valued sensors --- + 'wrist_pitch': { + 'val1': deque([0]*buffer_len, maxlen=buffer_len) # Only one line + }, + 'wrist_yaw': { + 'val1': deque([0]*buffer_len, maxlen=buffer_len) # Only one line + }, } -# Create a figure with 5 subplots (one for each sensor pair). -fig, axes = plt.subplots(5, 1, figsize=(8, 12), sharex=True) +# ------------------------------------------------------------------- +# 2) Figure with 7 subplots (was 5). We keep the original 5 + 2 new. +# ------------------------------------------------------------------- +fig, axes = plt.subplots(7, 1, figsize=(8, 14), sharex=True) fig.tight_layout(pad=3.0) -# We'll store line references in a dict so we can update them in our update() function. -lines = { - 'wrist_roll': [], - 'elbow_pitch': [], - 'shoulder_pitch': [], - 'shoulder_yaw': [], - 'shoulder_roll': [] -} +# We'll store line references in a dict so we can update them in update(). +lines = {} -# Set up each subplot +# ------------------------------------------------------------------- +# 3) Define each subplot, including new ones at the end. +# ------------------------------------------------------------------- subplot_info = [ - ('wrist_roll', 'Wrist Roll (2,3)', axes[0]), - ('elbow_pitch', 'Elbow Pitch (0,1)', axes[1]), - ('shoulder_pitch', 'Shoulder Pitch (10,11)', axes[2]), - ('shoulder_yaw', 'Shoulder Yaw (12,13)', axes[3]), - ('shoulder_roll', 'Shoulder Roll (14,15)', axes[4]) + ('wrist_roll', 'Wrist Roll (2,3)', axes[0]), + ('elbow_pitch', 'Elbow Pitch (0,1)', axes[1]), + ('shoulder_pitch', 'Shoulder Pitch (10,11)', axes[2]), + ('shoulder_yaw', 'Shoulder Yaw (12,13)', axes[3]), + ('shoulder_roll', 'Shoulder Roll (14,15)', axes[4]), + ('wrist_pitch', 'Wrist Pitch (0)', axes[5]), # new + ('wrist_yaw', 'Wrist Yaw (1)', axes[6]), # new ] +# Set up each subplot for (sensor_name, label, ax) in subplot_info: ax.set_title(label) ax.set_xlim(0, buffer_len) - ax.set_ylim(0, 4096) - line1, = ax.plot([], [], label=f"{sensor_name} - val1") - line2, = ax.plot([], [], label=f"{sensor_name} - val2") + ax.set_ylim(0, 4096) # adjust if needed + + # For existing sensors, plot 2 lines (val1, val2) + # For the new single-line sensors, plot just 1 line + if sensor_name in ['wrist_pitch', 'wrist_yaw']: + # Single-valued + line, = ax.plot([], [], label=f"{sensor_name}") + lines[sensor_name] = line + else: + # Pair of values + line1, = ax.plot([], [], label=f"{sensor_name} - val1") + line2, = ax.plot([], [], label=f"{sensor_name} - val2") + lines[sensor_name] = [line1, line2] + ax.legend() - lines[sensor_name] = [line1, line2] def update(frame): # Read all available lines from the serial buffer @@ -75,9 +94,8 @@ def update(frame): raw_line = ser.readline().decode('utf-8').strip() parts = raw_line.split() - # We expect at least 16 values if all sensors are present. - # (Because you mentioned indices 0..1, 2..3, 10..11, 12..13, 14..15) - if len(parts) < 16: + # We expect at least 16 values if all sensors are present + if len(parts) < 7: continue try: @@ -86,40 +104,54 @@ def update(frame): # If there's a parsing error, skip this line continue - # Extract the relevant values and append to the correct buffer - sensor_buffers['elbow_pitch']['val1'].append(values[0]) - sensor_buffers['elbow_pitch']['val2'].append(values[1]) + # Original code: extract the relevant values and append to the correct buffer + sensor_buffers['elbow_pitch']['val1'].append(values[13]) + sensor_buffers['elbow_pitch']['val2'].append(values[13]) - sensor_buffers['wrist_roll']['val1'].append(values[2]) + sensor_buffers['wrist_roll']['val1'].append(values[3]) sensor_buffers['wrist_roll']['val2'].append(values[3]) sensor_buffers['shoulder_pitch']['val1'].append(values[14]) - sensor_buffers['shoulder_pitch']['val2'].append(values[15]) + sensor_buffers['shoulder_pitch']['val2'].append(values[14]) - sensor_buffers['shoulder_yaw']['val1'].append(values[12]) - sensor_buffers['shoulder_yaw']['val2'].append(values[13]) + sensor_buffers['shoulder_yaw']['val1'].append(values[8]) + sensor_buffers['shoulder_yaw']['val2'].append(values[8]) sensor_buffers['shoulder_roll']['val1'].append(values[10]) - sensor_buffers['shoulder_roll']['val2'].append(values[11]) + sensor_buffers['shoulder_roll']['val2'].append(values[10]) + + # ------------------------------------------------------------------- + # 4) New code: also read wrist_pitch (index 0) and wrist_yaw (index 1) + # ------------------------------------------------------------------- + sensor_buffers['wrist_yaw']['val1'].append(values[0]) + sensor_buffers['wrist_pitch']['val1'].append(values[1]) # Update each line's data in each subplot all_lines = [] for (sensor_name, _, ax) in subplot_info: - # x-values are just the index range of the buffer + # x-values are just the index range of the buffer for val1 x_data = range(len(sensor_buffers[sensor_name]['val1'])) - # First line - lines[sensor_name][0].set_data( - x_data, - sensor_buffers[sensor_name]['val1'] - ) - # Second line - lines[sensor_name][1].set_data( - x_data, - sensor_buffers[sensor_name]['val2'] - ) - - all_lines.extend(lines[sensor_name]) + # If this sensor has two lines + if isinstance(lines[sensor_name], list): + # First line + lines[sensor_name][0].set_data( + x_data, + sensor_buffers[sensor_name]['val1'] + ) + # Second line + lines[sensor_name][1].set_data( + x_data, + sensor_buffers[sensor_name]['val2'] + ) + all_lines.extend(lines[sensor_name]) + else: + # Single line only (wrist_pitch, wrist_yaw) + lines[sensor_name].set_data( + x_data, + sensor_buffers[sensor_name]['val1'] + ) + all_lines.append(lines[sensor_name]) return all_lines diff --git a/examples/hopejr/follower.py b/examples/hopejr/follower.py index 83e8fe06..de385262 100644 --- a/examples/hopejr/follower.py +++ b/examples/hopejr/follower.py @@ -32,29 +32,29 @@ class HopeJuniorRobot: motors = { # Thumb "thumb_basel_rotation": [1, "scs0009"], - "thumb_mcp": [2, "scs0009"], - "thumb_pip": [3, "scs0009"], - "thumb_dip": [4, "scs0009"], + "thumb_mcp": [3, "scs0009"], + "thumb_pip": [4, "scs0009"], + "thumb_dip": [13, "scs0009"], # Index "index_thumb_side": [5, "scs0009"], "index_pinky_side": [6, "scs0009"], - "index_flexor": [7, "scs0009"], + "index_flexor": [16, "scs0009"], # Middle "middle_thumb_side": [8, "scs0009"], "middle_pinky_side": [9, "scs0009"], - "middle_flexor": [10, "scs0009"], + "middle_flexor": [2, "scs0009"], # Ring "ring_thumb_side": [11, "scs0009"], "ring_pinky_side": [12, "scs0009"], - "ring_flexor": [13, "scs0009"], + "ring_flexor": [7, "scs0009"], # Pinky "pinky_thumb_side": [14, "scs0009"], "pinky_pinky_side": [15, "scs0009"], - "pinky_flexor": [16, "scs0009"], + "pinky_flexor": [10, "scs0009"], }, protocol_version=1,#1 group_sync_read=False, @@ -83,48 +83,48 @@ class HopeJuniorRobot: start_pos = [ 750, # thumb_basel_rotation - 1000, # thumb_mcp - 500, # thumb_pip - 950, # thumb_dip + 100, # thumb_mcp + 700, # thumb_pip + 100, # thumb_dip - 150, # index_thumb_side + 800, # index_thumb_side 950, # index_pinky_side - 500, # index_flexor + 0, # index_flexor 250, # middle_thumb_side 850, # middle_pinky_side - 1000, # middle_flexor + 0, # middle_flexor 850, # ring_thumb_side 900, # ring_pinky_side - 600, # ring_flexor + 0, # ring_flexor 00, # pinky_thumb_side 950, # pinky_pinky_side - 150, # pinky_flexor + 0, # pinky_flexor ] end_pos = [ start_pos[0] - 550, # thumb_basel_rotation - start_pos[1] - 350, # thumb_mcp - start_pos[2] + 500, # thumb_pip - start_pos[3] - 550, # thumb_dip + start_pos[1] + 400, # thumb_mcp + start_pos[2] + 300, # thumb_pip + start_pos[3] + 200, # thumb_dip - start_pos[4] + 350, # index_thumb_side + start_pos[4] - 700, # index_thumb_side start_pos[5] - 300, # index_pinky_side - start_pos[6] + 500, # index_flexor + start_pos[6] + 600, # index_flexor - start_pos[7] + 400, # middle_thumb_side + start_pos[7] + 700, # middle_thumb_side start_pos[8] - 400, # middle_pinky_side - start_pos[9] - 650, # middle_flexor + start_pos[9] + 600, # middle_flexor - start_pos[10] - 400, # ring_thumb_side + start_pos[10] - 600, # ring_thumb_side start_pos[11] - 400, # ring_pinky_side - start_pos[12] + 400, # ring_flexor + start_pos[12] + 600, # ring_flexor - start_pos[13] + 500, # pinky_thumb_side - start_pos[14] - 350, # pinky_pinky_side - start_pos[15] + 450, # pinky_flexor + start_pos[13] + 400, # pinky_thumb_side + start_pos[14] - 450, # pinky_pinky_side + start_pos[15] + 600, # pinky_flexor ] @@ -148,23 +148,23 @@ class HopeJuniorRobot: drive_mode = [0] * len(self.arm_bus.motor_names) start_pos = [ - 1600, # shoulder_up - 2450, # shoulder_forward - 1700, # shoulder_roll - 1150, # bend_elbow + 1800, # shoulder_up + 2800, # shoulder_forward + 1800, # shoulder_roll + 1200, # bend_elbow 700, # wrist_roll 1850, # wrist_yaw 1700, # wrist_pitch ] end_pos = [ - 3100, # shoulder_up + 2800, # shoulder_up 3150, # shoulder_forward 400, #shoulder_roll - 2800, # bend_elbow - 2600, # wrist_roll + 2300, # bend_elbow + 2300, # wrist_roll 2150, # wrist_yaw - 2400, # wrist_pitch + 2300, # wrist_pitch ] calib_modes = [CalibrationMode.LINEAR.name] * len(self.arm_bus.motor_names) diff --git a/examples/hopejr/read_arm.cs b/examples/hopejr/read_arm.cs new file mode 100644 index 00000000..459dcdbf --- /dev/null +++ b/examples/hopejr/read_arm.cs @@ -0,0 +1,52 @@ +#include + +// Define multiplexer input pins +#define S0 5 +#define S1 6 +#define S2 8 +#define S3 7 +#define SENSOR_INPUT 4 + +#define SENSOR_COUNT 16 + +int rawVals[SENSOR_COUNT]; + +void measureRawValues() { + for (uint8_t i = 0; i < SENSOR_COUNT; i++) { + digitalWrite(S0, (i & 0b1) ^ 0b1);; + digitalWrite(S1, (i >> 1 & 0b1) ^ 0b1);; + digitalWrite(S2, (i >> 2 & 0b1) ^ 0b1);; + digitalWrite(S3, i >> 3 & 0b1); + delay(1); + + rawVals[i] = analogRead(SENSOR_INPUT); + } +} + +void printRawValues() { + for (uint8_t i = 0; i < SENSOR_COUNT; i++) { + Serial.print(rawVals[i]); + if (i < SENSOR_COUNT - 1) Serial.print(" "); + } + Serial.println(); +} + +void setup() { + Serial.begin(115200); + + pinMode(S0, OUTPUT); + pinMode(S1, OUTPUT); + pinMode(S2, OUTPUT); + pinMode(S3, OUTPUT); + + digitalWrite(S0, LOW); + digitalWrite(S1, LOW); + digitalWrite(S2, LOW); + digitalWrite(S3, LOW); +} + +void loop() { + measureRawValues(); + printRawValues(); + delay(1); +} diff --git a/examples/hopejr/read_glove.cs b/examples/hopejr/read_glove.cs new file mode 100644 index 00000000..459dcdbf --- /dev/null +++ b/examples/hopejr/read_glove.cs @@ -0,0 +1,52 @@ +#include + +// Define multiplexer input pins +#define S0 5 +#define S1 6 +#define S2 8 +#define S3 7 +#define SENSOR_INPUT 4 + +#define SENSOR_COUNT 16 + +int rawVals[SENSOR_COUNT]; + +void measureRawValues() { + for (uint8_t i = 0; i < SENSOR_COUNT; i++) { + digitalWrite(S0, (i & 0b1) ^ 0b1);; + digitalWrite(S1, (i >> 1 & 0b1) ^ 0b1);; + digitalWrite(S2, (i >> 2 & 0b1) ^ 0b1);; + digitalWrite(S3, i >> 3 & 0b1); + delay(1); + + rawVals[i] = analogRead(SENSOR_INPUT); + } +} + +void printRawValues() { + for (uint8_t i = 0; i < SENSOR_COUNT; i++) { + Serial.print(rawVals[i]); + if (i < SENSOR_COUNT - 1) Serial.print(" "); + } + Serial.println(); +} + +void setup() { + Serial.begin(115200); + + pinMode(S0, OUTPUT); + pinMode(S1, OUTPUT); + pinMode(S2, OUTPUT); + pinMode(S3, OUTPUT); + + digitalWrite(S0, LOW); + digitalWrite(S1, LOW); + digitalWrite(S2, LOW); + digitalWrite(S3, LOW); +} + +void loop() { + measureRawValues(); + printRawValues(); + delay(1); +} diff --git a/examples/hopejr/settings/config.yaml b/examples/hopejr/settings/config.yaml index 3d4bccfa..1416ef36 100644 --- a/examples/hopejr/settings/config.yaml +++ b/examples/hopejr/settings/config.yaml @@ -10,6 +10,6 @@ robot: Protective_Torque: 1 Maximum_Acceleration: 100 Torque_Enable: 1 - Acceleration: 100 + Acceleration: 30 hand_bus: Acceleration: 100 \ No newline at end of file diff --git a/examples/hopejr/teleop.py b/examples/hopejr/teleop.py index 6c92dbea..ce8e1022 100644 --- a/examples/hopejr/teleop.py +++ b/examples/hopejr/teleop.py @@ -23,45 +23,45 @@ def main( robot.connect_hand() - #robot.connect_arm() + robot.connect_arm() #read pos print(robot.hand_bus.read("Present_Position")) - #print(robot.arm_bus.read("Present_Position", "shoulder_pitch")) - #print(robot.arm_bus.read("Present_Position",["shoulder_yaw","shoulder_roll","elbow_flex","wrist_roll","wrist_yaw","wrist_pitch"])) - - #for i in range(10): - # time.sleep(0.1) - # robot.apply_arm_config('examples/hopejr/settings/config.yaml') + print(robot.arm_bus.read("Present_Position", "shoulder_pitch")) + print(robot.arm_bus.read("Present_Position",["shoulder_yaw","shoulder_roll","elbow_flex","wrist_roll","wrist_yaw","wrist_pitch"])) + #robot.arm_bus.write("Goal_Position", robot.arm_calib_dict["start_pos"][0]*1 +robot.arm_calib_dict["end_pos"][0]*0, ["wrist_roll"]) + for i in range(10): + time.sleep(0.1) + robot.apply_arm_config('examples/hopejr/settings/config.yaml') # #calibrate arm - #arm_calibration = robot.get_arm_calibration() - #exoskeleton = HomonculusArm(serial_port="/dev/ttyACM2") - #robot.arm_bus.write("Goal_Position", robot.arm_calib_dict["start_pos"][0]*0.7 + robot.arm_calib_dict["end_pos"][0]*0.3, ["shoulder_pitch"]) - - #if calibrate_exoskeleton: - # exoskeleton.run_calibration(robot) + arm_calibration = robot.get_arm_calibration() + exoskeleton = HomonculusArm(serial_port="/dev/ttyACM2") - #file_path = "examples/hopejr/settings/arm_calib.pkl" - #with open(file_path, "rb") as f: - # calib_dict = pickle.load(f) - #print("Loaded dictionary:", calib_dict) - #exoskeleton.set_calibration(calib_dict) + + if calibrate_exoskeleton: + exoskeleton.run_calibration(robot) + + file_path = "examples/hopejr/settings/arm_calib.pkl" + with open(file_path, "rb") as f: + calib_dict = pickle.load(f) + print("Loaded dictionary:", calib_dict) + exoskeleton.set_calibration(calib_dict) #calibrate hand hand_calibration = robot.get_hand_calibration() - glove = HomonculusGlove(serial_port = "/dev/ttyACM2") + glove = HomonculusGlove(serial_port = "/dev/ttyACM0") if calibrate_glove: - glove.run_calibration() + glove.run_calibration() file_path = "examples/hopejr/settings/hand_calib.pkl" with open(file_path, "rb") as f: - calib_dict = pickle.load(f) + calib_dict = pickle.load(f) print("Loaded dictionary:", calib_dict) glove.set_calibration(calib_dict) robot.hand_bus.set_calibration(hand_calibration) - #robot.arm_bus.set_calibration(arm_calibration) + robot.arm_bus.set_calibration(arm_calibration) # Initialize Pygame # pygame.init() @@ -96,23 +96,30 @@ def main( # hand_components.append({"pos": (x, y + i * 50), "value": 0}) for i in range(1000000000000000): - # robot.apply_arm_config('examples/hopejr/settings//config.yaml') - # robot.arm_bus.write("Acceleration", 50, "shoulder_yaw") - # joint_names = ["shoulder_pitch", "shoulder_yaw", "shoulder_roll", "elbow_flex", "wrist_roll", "wrist_yaw", "wrist_pitch"] - # joint_values = exoskeleton.read_running_average(motor_names=joint_names, linearize=True) + robot.apply_arm_config('examples/hopejr/settings/config.yaml') + #robot.arm_bus.write("Acceleration", 50, "shoulder_yaw") + joint_names = ["shoulder_pitch", "shoulder_yaw", "shoulder_roll", "elbow_flex", "wrist_roll", "wrist_yaw", "wrist_pitch"] + #only wrist roll + #joint_names = ["shoulder_pitch"] + joint_values = exoskeleton.read(motor_names=joint_names) - # joint_values = joint_values.round().astype(int) - # joint_dict = {k: v for k, v in zip(joint_names, joint_values, strict=False)} + #joint_values = joint_values.round().astype(int) + joint_dict = {k: v for k, v in zip(joint_names, joint_values, strict=False)} - # motor_values = [] - # motor_names = [] - # motor_names += ["shoulder_pitch", "shoulder_yaw", "shoulder_roll", "elbow_flex", "wrist_roll", "wrist_yaw", "wrist_pitch"] - # motor_values += [joint_dict[name]-30 for name in motor_names] + motor_values = [] + motor_names = [] + motor_names += ["shoulder_pitch", "shoulder_yaw", "shoulder_roll", "elbow_flex", "wrist_roll", "wrist_yaw", "wrist_pitch"] + #motor_names += ["shoulder_pitch"] + motor_values += [joint_dict[name] for name in motor_names] + #remove 50 from shoulder_roll + #motor_values += [joint_dict[name] for name in motor_names] - # motor_values = np.array(motor_values) - # motor_values = np.clip(motor_values, 0, 100) - # if not freeze_arm: - # robot.arm_bus.write("Goal_Position", motor_values, motor_names) + motor_values = np.array(motor_values) + motor_values = np.clip(motor_values, 0, 100) + + print(motor_names, motor_values) + if not freeze_arm: + robot.arm_bus.write("Goal_Position", motor_values, motor_names) if not freeze_fingers:#include hand hand_joint_names = [] diff --git a/examples/test_torque/faulty_servo.py b/examples/test_torque/faulty_servo.py new file mode 100644 index 00000000..8986a329 --- /dev/null +++ b/examples/test_torque/faulty_servo.py @@ -0,0 +1,97 @@ +#faulty servo +Model = [777] +ID = [7] +Baud_Rate = [0] +Return_Delay = [0] +Response_Status_Level = [1] +Min_Angle_Limit = [1140] +Max_Angle_Limit = [2750] +Max_Temperature_Limit = [70] +Max_Voltage_Limit = [140] +Min_Voltage_Limit = [40] +Max_Torque_Limit = [1000] +Phase = [12] +Unloading_Condition = [44] +LED_Alarm_Condition = [47] +P_Coefficient = [32] +D_Coefficient = [32] +I_Coefficient = [0] +Minimum_Startup_Force = [16] +CW_Dead_Zone = [1] +CCW_Dead_Zone = [1] +Protection_Current = [310] +Angular_Resolution = [1] +Offset = [1047] +Mode = [0] +Protective_Torque = [20] +Protection_Time = [200] +Overload_Torque = [80] +Speed_closed_loop_P_proportional_coefficient = [10] +Over_Current_Protection_Time = [200] +Velocity_closed_loop_I_integral_coefficient = [200] +Torque_Enable = [1] +Acceleration = [20] +Goal_Position = [0] +Goal_Time = [0] +Goal_Speed = [0] +Torque_Limit = [1000] +Lock = [1] +Present_Position = [1494] +Present_Speed = [0] +Present_Load = [0] +Present_Voltage = [123] +Present_Temperature = [24] +Status = [0] +Moving = [0] +Present_Current = [0] +Maximum_Acceleration = [306] + + + +#all servos of hopejr +Model = [2825 777 777 2825 777 777 777] +ID = [1 2 3 4 5 6 7] +Baud_Rate = [0 0 0 0 0 0 0] +Return_Delay = [0 0 0 0 0 0 0] +Response_Status_Level = [1 1 1 1 1 1 1] +Min_Angle_Limit = [ 650 1300 1300 1200 600 1725 0] +Max_Angle_Limit = [2600 2050 2800 2500 4096 2250 4095] +Max_Temperature_Limit = [80 70 70 80 70 70 70] +Max_Voltage_Limit = [160 140 140 160 140 140 80] +Min_Voltage_Limit = [60 40 40 60 40 40 40] +Max_Torque_Limit = [1000 1000 1000 1000 1000 1000 1000] +Phase = [12 12 12 12 12 12 12] +Unloading_Condition = [45 44 44 45 44 44 44] +LED_Alarm_Condition = [45 47 47 45 47 47 47] +P_Coefficient = [32 32 32 32 32 32 32] +D_Coefficient = [32 32 32 32 32 32 32] +I_Coefficient = [0 0 0 0 0 0 0] +Minimum_Startup_Force = [15 16 16 12 16 16 16] +CW_Dead_Zone = [0 1 1 0 1 1 1] +CCW_Dead_Zone = [0 1 1 0 1 1 1] +Protection_Current = [310 310 310 310 310 310 500] +Angular_Resolution = [1 1 1 1 1 1 1] +Offset = [ 0 1047 1024 1047 1024 1024 0] +Mode = [0 0 0 0 0 0 0] +Protective_Torque = [20 20 20 20 20 20 20] +Protection_Time = [200 200 200 200 200 200 200] +Overload_Torque = [80 80 80 80 80 80 80] +Speed_closed_loop_P_proportional_coefficient = [10 10 10 10 10 10 10] +Over_Current_Protection_Time = [250 200 200 250 200 200 200] +Velocity_closed_loop_I_integral_coefficient = [200 200 200 200 200 200 200] +Torque_Enable = [1 1 1 1 1 1 1] +Acceleration = [20 20 20 20 20 20 20] +Goal_Position = [1909 1977 1820 1000 707 1941 1036] +Goal_Time = [0 0 0 0 0 0 0] +Goal_Speed = [0 0 0 0 0 0 0] +Torque_Limit = [1000 1000 1000 200 1000 1000 1000] +Lock = [1 1 1 1 1 1 1] +Present_Position = [1909 1982 1821 1200 710 1941 1036] +Present_Speed = [0 0 0 0 0 0 0] +Present_Load = [ 0 48 0 0 32 0 0] +Present_Voltage = [122 123 122 123 122 122 122] +Present_Temperature = [23 28 28 26 29 28 28] +Status = [0 0 0 0 0 0 1] +Moving = [0 0 0 0 0 0 0] +Present_Current = [0 1 0 1 1 0 1] +Maximum_Acceleration = [1530 306 306 1530 306 306 254] \ No newline at end of file diff --git a/examples/test_torque/hopejr.py b/examples/test_torque/hopejr.py new file mode 100644 index 00000000..d71df78f --- /dev/null +++ b/examples/test_torque/hopejr.py @@ -0,0 +1,192 @@ +import threading +import time +from typing import Callable + +import cv2 +import numpy as np +import serial + +from lerobot.common.robot_devices.motors.feetech import ( + CalibrationMode, + FeetechMotorsBus, +) + +LOWER_BOUND_LINEAR = -100 +UPPER_BOUND_LINEAR = 200 + +ESCAPE_KEY_ID = 27 + + +class HopeJuniorRobot: + def __init__(self): + self.arm_bus = FeetechMotorsBus( + port="/dev/ttyACM1", + motors={ + # "motor1": (2, "sts3250"), + # "motor2": (1, "scs0009"), + #"shoulder_pitch": [1, "sts3250"], + #"shoulder_yaw": [2, "sts3215"], # TODO: sts3250 + #"shoulder_roll": [3, "sts3215"], # TODO: sts3250 + #"elbow_flex": [4, "sts3250"], + #"wrist_roll": [5, "sts3215"], + #"wrist_yaw": [6, "sts3215"], + "wrist_pitch": [7, "sts3215"], + }, + protocol_version=0, + ) + self.hand_bus = FeetechMotorsBus( + port="/dev/ttyACM1", + motors={ + "thumb_basel_rotation": [30, "scs0009"], + "thumb_flexor": [27, "scs0009"], + "thumb_pinky_side": [26, "scs0009"], + "thumb_thumb_side": [28, "scs0009"], + "index_flexor": [25, "scs0009"], + "index_pinky_side": [31, "scs0009"], + "index_thumb_side": [32, "scs0009"], + "middle_flexor": [24, "scs0009"], + "middle_pinky_side": [33, "scs0009"], + "middle_thumb_side": [34, "scs0009"], + "ring_flexor": [21, "scs0009"], + "ring_pinky_side": [36, "scs0009"], + "ring_thumb_side": [35, "scs0009"], + "pinky_flexor": [23, "scs0009"], + "pinky_pinky_side": [38, "scs0009"], + "pinky_thumb_side": [37, "scs0009"], + }, + protocol_version=1, + group_sync_read=False, + ) + + def get_hand_calibration(self): + """ + Returns a dictionary containing calibration settings for each motor + on the hand bus. + """ + homing_offset = [0] * len(self.hand_bus.motor_names) + drive_mode = [0] * len(self.hand_bus.motor_names) + + start_pos = [ + 500, 900, 0, 1000, 100, 250, 750, 100, 400, 150, 100, 120, 980, 100, 950, 750, + ] + + end_pos = [ + start_pos[0] - 400, # 500 - 400 = 100 + start_pos[1] - 300, # 900 - 300 = 600 + start_pos[2] + 550, # 0 + 550 = 550 + start_pos[3] - 550, # 1000 - 550 = 450 + start_pos[4] + 900, # 100 + 900 = 1000 + start_pos[5] + 500, # 250 + 500 = 750 + start_pos[6] - 500, # 750 - 500 = 250 + start_pos[7] + 900, # 100 + 900 = 1000 + start_pos[8] + 700, # 400 + 700 = 1100 + start_pos[9] + 700, # 150 + 700 = 850 + start_pos[10] + 900, # 100 + 900 = 1000 + start_pos[11] + 700, # 120 + 700 = 820 + start_pos[12] - 700, # 980 - 700 = 280 + start_pos[13] + 900, # 100 + 900 = 1000 + start_pos[14] - 700, # 950 - 700 = 250 + start_pos[15] - 700, # 750 - 700 = 50 + ] + + calib_modes = [CalibrationMode.LINEAR.name] * len(self.hand_bus.motor_names) + + calib_dict = { + "homing_offset": homing_offset, + "drive_mode": drive_mode, + "start_pos": start_pos, + "end_pos": end_pos, + "calib_mode": calib_modes, + "motor_names": self.hand_bus.motor_names, + } + return calib_dict + + def get_arm_calibration(self): + """ + Returns a dictionary containing calibration settings for each motor + on the arm bus. + """ + homing_offset = [0] * len(self.arm_bus.motor_names) + drive_mode = [0] * len(self.arm_bus.motor_names) + + # Example placeholders + start_pos = [ + 600, # shoulder_up + 1500, # shoulder_forward + 1300, # shoulder_yaw + 1000, # bend_elbow + 1600, # wrist_roll + 1700, # wrist_yaw + 600, # wrist_pitch + ] + + end_pos = [ + 2300, # shoulder_up + 2300, # shoulder_forward + 2800, # shoulder_yaw + 2500, # bend_elbow + 2800, # wrist_roll + 2200, # wrist_yaw + 1700, # wrist_pitch + ] + + calib_modes = [CalibrationMode.LINEAR.name] * len(self.arm_bus.motor_names) + + calib_dict = { + "homing_offset": homing_offset, + "drive_mode": drive_mode, + "start_pos": start_pos, + "end_pos": end_pos, + "calib_mode": calib_modes, + "motor_names": self.arm_bus.motor_names, + } + return calib_dict + + def connect(self): + """Connect to the Feetech buses.""" + self.arm_bus.connect() + # self.hand_bus.connect() + + +def capture_and_display_processed_frames( + frame_processor: Callable[[np.ndarray], np.ndarray], + window_display_name: str, + cap_device: int = 0, +) -> None: + """ + Capture frames from the given input camera device, run them through + the frame processor, and display the outputs in a window with the given name. + + User should press Esc to exit. + + Inputs: + frame_processor: Callable[[np.ndarray], np.ndarray] + Processes frames. + Input and output are numpy arrays of shape (H W C) with BGR channel layout and dtype uint8 / byte. + window_display_name: str + Name of the window used to display frames. + cap_device: int + Identifier for the camera to use to capture frames. + """ + cv2.namedWindow(window_display_name) + capture = cv2.VideoCapture(cap_device) + if not capture.isOpened(): + raise ValueError("Unable to open video capture.") + + frame_count = 0 + has_frame, frame = capture.read() + while has_frame: + frame_count = frame_count + 1 + # Mirror frame horizontally and flip color for demonstration + frame = np.ascontiguousarray(frame[:, ::-1, ::-1]) + + # process & show frame + processed_frame = frame_processor(frame) + cv2.imshow(window_display_name, processed_frame[:, :, ::-1]) + + has_frame, frame = capture.read() + key = cv2.waitKey(1) + if key == ESCAPE_KEY_ID: + break + + capture.release() diff --git a/examples/test_torque/log_50ms_shoulder_pitch.png b/examples/test_torque/log_50ms_shoulder_pitch.png new file mode 100644 index 00000000..f46071cd Binary files /dev/null and b/examples/test_torque/log_50ms_shoulder_pitch.png differ diff --git a/examples/test_torque/log_50ms_shoulder_pitch_CURRENT.png b/examples/test_torque/log_50ms_shoulder_pitch_CURRENT.png new file mode 100644 index 00000000..2a5ef824 Binary files /dev/null and b/examples/test_torque/log_50ms_shoulder_pitch_CURRENT.png differ diff --git a/examples/test_torque/log_50ms_shoulder_pitch_LOAD.png b/examples/test_torque/log_50ms_shoulder_pitch_LOAD.png new file mode 100644 index 00000000..f76ac8d7 Binary files /dev/null and b/examples/test_torque/log_50ms_shoulder_pitch_LOAD.png differ diff --git a/examples/test_torque/log_and_plot_feetech.py b/examples/test_torque/log_and_plot_feetech.py new file mode 100644 index 00000000..d2af5390 --- /dev/null +++ b/examples/test_torque/log_and_plot_feetech.py @@ -0,0 +1,44 @@ + + +import matplotlib.pyplot as plt +import time +from typing import List, Tuple +def log_and_plot_params(bus, params_to_log: list, servo_names: list, + test_id="servo_log", interval=0.1, duration=5, save_plot=True) -> Tuple[dict, List[float]]: + + """ + Logs specific servo parameters for a given duration and generates a plot. + """ + + servo_data = {servo_name: {param: [] for param in params_to_log} for servo_name in servo_names} + timestamps = [] + + start_time = time.time() + + while time.time() - start_time < duration: + timestamp = time.time() - start_time + timestamps.append(timestamp) + for param in params_to_log: + values = bus.read(param, servo_names) + for servo_name, value in zip(servo_names, values): + servo_data[servo_name][param].append(value) + + time.sleep(interval) + + if save_plot: + for servo_name, data in servo_data.items(): + plt.figure(figsize=(10, 6)) + for param in params_to_log: + if all(v is not None for v in data[param]): + plt.plot(timestamps, data[param], label=param) + plt.xlabel("Time (s)") + plt.ylabel("Parameter Values") + plt.title(f"Parameter Trends for Servo: {servo_name}") + plt.legend() + plt.grid(True) + plt.tight_layout() + plot_filename = f"{test_id}_{servo_name}.png" + plt.savefig(plot_filename) + print(f"Plot saved as {plot_filename}") + + return servo_data, timestamps diff --git a/examples/test_torque/plots/log_200ms_shoulder_pitch_current.png b/examples/test_torque/plots/log_200ms_shoulder_pitch_current.png new file mode 100644 index 00000000..b8630ccb Binary files /dev/null and b/examples/test_torque/plots/log_200ms_shoulder_pitch_current.png differ diff --git a/examples/test_torque/plots/log_50ms_shoulder_pitch.png b/examples/test_torque/plots/log_50ms_shoulder_pitch.png new file mode 100644 index 00000000..6e05198f Binary files /dev/null and b/examples/test_torque/plots/log_50ms_shoulder_pitch.png differ diff --git a/examples/test_torque/plots/log_50ms_shoulder_pitch_current.png b/examples/test_torque/plots/log_50ms_shoulder_pitch_current.png new file mode 100644 index 00000000..8229a4a6 Binary files /dev/null and b/examples/test_torque/plots/log_50ms_shoulder_pitch_current.png differ diff --git a/examples/test_torque/print_all_params.py b/examples/test_torque/print_all_params.py new file mode 100644 index 00000000..41c9ccdb --- /dev/null +++ b/examples/test_torque/print_all_params.py @@ -0,0 +1,68 @@ +STS_SERIES_CONTROL_TABLE = { + "Model": (3, 2), + "ID": (5, 1), + "Baud_Rate": (6, 1), + "Return_Delay": (7, 1), + "Response_Status_Level": (8, 1), + "Min_Angle_Limit": (9, 2), + "Max_Angle_Limit": (11, 2), + "Max_Temperature_Limit": (13, 1), + "Max_Voltage_Limit": (14, 1), + "Min_Voltage_Limit": (15, 1), + "Max_Torque_Limit": (16, 2), + "Phase": (18, 1), + "Unloading_Condition": (19, 1), + "LED_Alarm_Condition": (20, 1), + "P_Coefficient": (21, 1), + "D_Coefficient": (22, 1), + "I_Coefficient": (23, 1), + "Minimum_Startup_Force": (24, 2), + "CW_Dead_Zone": (26, 1), + "CCW_Dead_Zone": (27, 1), + "Protection_Current": (28, 2), + "Angular_Resolution": (30, 1), + "Offset": (31, 2), + "Mode": (33, 1), + "Protective_Torque": (34, 1), + "Protection_Time": (35, 1), + "Overload_Torque": (36, 1), + "Speed_closed_loop_P_proportional_coefficient": (37, 1), + "Over_Current_Protection_Time": (38, 1), + "Velocity_closed_loop_I_integral_coefficient": (39, 1), + "Torque_Enable": (40, 1), + "Acceleration": (41, 1), + "Goal_Position": (42, 2), + "Goal_Time": (44, 2), + "Goal_Speed": (46, 2), + "Torque_Limit": (48, 2), + "Lock": (55, 1), + "Present_Position": (56, 2), + "Present_Speed": (58, 2), + "Present_Load": (60, 2), + "Present_Voltage": (62, 1), + "Present_Temperature": (63, 1), + "Status": (65, 1), + "Moving": (66, 1), + "Present_Current": (69, 2), + # Not in the Memory Table + "Maximum_Acceleration": (85, 2), +} + +import time + +# Assuming STS_SERIES_CONTROL_TABLE is defined globally + +def print_all_params(robot): + """ + Reads all parameters from the STS_SERIES_CONTROL_TABLE and prints their values. + """ + for param in STS_SERIES_CONTROL_TABLE.keys(): + try: + val = robot.arm_bus.read(param) + print(f"{param} = {val}") + except Exception as e: + print(f"{param} read failed: {e}") + + +# Example usage: +print_all_params(robot) diff --git a/examples/test_torque/read_encoder.cs b/examples/test_torque/read_encoder.cs new file mode 100644 index 00000000..e7a5b4e8 --- /dev/null +++ b/examples/test_torque/read_encoder.cs @@ -0,0 +1,26 @@ +#include + +DFRobot_VisualRotaryEncoder_I2C sensor(0x54, &Wire); + +void setup() +{ + Serial.begin(115200); + + // Attempt to initialize the sensor + while (NO_ERR != sensor.begin()) { + // Failed? Just wait a bit and try again + delay(3000); + } +} + +void loop() +{ + // Read the encoder value + uint16_t encoderValue = sensor.getEncoderValue(); + + // Print it followed by a newline + Serial.println(encoderValue); + + // Delay 10ms between readings + delay(10); +} diff --git a/examples/test_torque/test_torque.ipynb b/examples/test_torque/test_torque.ipynb new file mode 100644 index 00000000..6b459c22 --- /dev/null +++ b/examples/test_torque/test_torque.ipynb @@ -0,0 +1,544 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "what are the actual interest values on the hopejr? make like a map" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "can change these dynamically so if the arm is moving down we can relax it instead of tensing it? so for example decreasing torque if the target position is lower than the actual position, for example. " + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "arm_calibration = robot.get_arm_calibration()\n", + "robot.arm_bus.write(\"Goal_Position\", arm_calibration[\"start_pos\"])" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Present Position: [1494]\n", + "Acceleration Read: [20]\n" + ] + } + ], + "source": [ + "import time\n", + "from hopejr import HopeJuniorRobot\n", + "\n", + "\n", + "robot = HopeJuniorRobot()\n", + "robot.connect()\n", + "\n", + "# Example read of the current position\n", + "print(\"Present Position:\", robot.arm_bus.read(\"Present_Position\"))\n", + "\n", + "# Enable torque and set acceleration\n", + "robot.arm_bus.write(\"Torque_Enable\", 1)\n", + "robot.arm_bus.write(\"Acceleration\", 20)\n", + "print(\"Acceleration Read:\", robot.arm_bus.read(\"Acceleration\"))\n" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [], + "source": [ + "robot.arm_bus.write(\"Torque_Limit\", 100,[\"elbow_flex\"])\n", + "robot.arm_bus.write(\"Protective_Torque\", 0, [\"elbow_flex\"])\n", + "robot.arm_bus.write(\"Acceleration\", 20)\n", + "robot.arm_bus.write(\"Goal_Position\", [2000], [\"elbow_flex\"])" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "array([1000, 1000, 1000, 1000, 1000, 1000, 1000])" + ] + }, + "execution_count": 8, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "robot.arm_bus.read(\"Max_Torque_Limit\")" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [], + "source": [ + "robot.arm_bus.write(\"Goal_Position\", [1909, 1977, 1820, 1000, 707, 1941, 1036]) #" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "robot.arm_bus.write(\"Max_Voltage_Limit\", [160, 140, 140, 160, 140, 140, 140]) #so its not torque limit nor max torque limit, , no protective torque or overload torque\n", + "#it's 1) max voltage limit, min-max angle limits are arbitrairly set for all the motors, max temp is only set for the shoulder\n", + "#max acceleration is also set, we could lower that in the elbow to make it less responsive to commands basically\n", + "#so we limit speed and temperature, maybe we should limit torque thouhg, minimum startup force is also important. protection current as well\n", + "#changed that to 310.\n", + "#\"Max_Voltage_Limit\" also needs to be changed, different from torque_limit" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Model = [777]\n", + "ID = [7]\n", + "Baud_Rate = [0]\n", + "Return_Delay = [0]\n", + "Response_Status_Level = [1]\n", + "Min_Angle_Limit = [1140]\n", + "Max_Angle_Limit = [2750]\n", + "Max_Temperature_Limit = [70]\n", + "Max_Voltage_Limit = [140]\n", + "Min_Voltage_Limit = [40]\n", + "Max_Torque_Limit = [1000]\n", + "Phase = [12]\n", + "Unloading_Condition = [44]\n", + "LED_Alarm_Condition = [47]\n", + "P_Coefficient = [32]\n", + "D_Coefficient = [32]\n", + "I_Coefficient = [0]\n", + "Minimum_Startup_Force = [16]\n", + "CW_Dead_Zone = [1]\n", + "CCW_Dead_Zone = [1]\n", + "Protection_Current = [310]\n", + "Angular_Resolution = [1]\n", + "Offset = [1047]\n", + "Mode = [0]\n", + "Protective_Torque = [20]\n", + "Protection_Time = [200]\n", + "Overload_Torque = [80]\n", + "Speed_closed_loop_P_proportional_coefficient = [10]\n", + "Over_Current_Protection_Time = [200]\n", + "Velocity_closed_loop_I_integral_coefficient = [200]\n", + "Torque_Enable = [1]\n", + "Acceleration = [20]\n", + "Goal_Position = [0]\n", + "Goal_Time = [0]\n", + "Goal_Speed = [0]\n", + "Torque_Limit = [1000]\n", + "Lock = [1]\n", + "Present_Position = [1494]\n", + "Present_Speed = [0]\n", + "Present_Load = [0]\n", + "Present_Voltage = [123]\n", + "Present_Temperature = [24]\n", + "Status = [0]\n", + "Moving = [0]\n", + "Present_Current = [0]\n", + "Maximum_Acceleration = [306]\n" + ] + } + ], + "source": [ + "STS_SERIES_CONTROL_TABLE = {\n", + " \"Model\": (3, 2),\n", + " \"ID\": (5, 1),\n", + " \"Baud_Rate\": (6, 1),\n", + " \"Return_Delay\": (7, 1),\n", + " \"Response_Status_Level\": (8, 1),\n", + " \"Min_Angle_Limit\": (9, 2),\n", + " \"Max_Angle_Limit\": (11, 2),\n", + " \"Max_Temperature_Limit\": (13, 1),\n", + " \"Max_Voltage_Limit\": (14, 1),\n", + " \"Min_Voltage_Limit\": (15, 1),\n", + " \"Max_Torque_Limit\": (16, 2),\n", + " \"Phase\": (18, 1),\n", + " \"Unloading_Condition\": (19, 1),\n", + " \"LED_Alarm_Condition\": (20, 1),\n", + " \"P_Coefficient\": (21, 1),\n", + " \"D_Coefficient\": (22, 1),\n", + " \"I_Coefficient\": (23, 1),\n", + " \"Minimum_Startup_Force\": (24, 2),\n", + " \"CW_Dead_Zone\": (26, 1),\n", + " \"CCW_Dead_Zone\": (27, 1),\n", + " \"Protection_Current\": (28, 2),\n", + " \"Angular_Resolution\": (30, 1),\n", + " \"Offset\": (31, 2),\n", + " \"Mode\": (33, 1),\n", + " \"Protective_Torque\": (34, 1),\n", + " \"Protection_Time\": (35, 1),\n", + " \"Overload_Torque\": (36, 1),\n", + " \"Speed_closed_loop_P_proportional_coefficient\": (37, 1),\n", + " \"Over_Current_Protection_Time\": (38, 1),\n", + " \"Velocity_closed_loop_I_integral_coefficient\": (39, 1),\n", + " \"Torque_Enable\": (40, 1),\n", + " \"Acceleration\": (41, 1),\n", + " \"Goal_Position\": (42, 2),\n", + " \"Goal_Time\": (44, 2),\n", + " \"Goal_Speed\": (46, 2),\n", + " \"Torque_Limit\": (48, 2),\n", + " \"Lock\": (55, 1),\n", + " \"Present_Position\": (56, 2),\n", + " \"Present_Speed\": (58, 2),\n", + " \"Present_Load\": (60, 2),\n", + " \"Present_Voltage\": (62, 1),\n", + " \"Present_Temperature\": (63, 1),\n", + " \"Status\": (65, 1),\n", + " \"Moving\": (66, 1),\n", + " \"Present_Current\": (69, 2),\n", + " # Not in the Memory Table\n", + " \"Maximum_Acceleration\": (85, 2),\n", + "}\n", + "\n", + "import time\n", + "\n", + "# Assuming STS_SERIES_CONTROL_TABLE is defined globally\n", + "\n", + "def print_all_params(robot):\n", + " \"\"\"\n", + " Reads all parameters from the STS_SERIES_CONTROL_TABLE and prints their values.\n", + " \"\"\"\n", + " for param in STS_SERIES_CONTROL_TABLE.keys():\n", + " try:\n", + " val = robot.arm_bus.read(param)\n", + " print(f\"{param} = {val}\")\n", + " except Exception as e:\n", + " print(f\"{param} read failed: {e}\")\n", + "\n", + "\n", + "# Example usage:\n", + "print_all_params(robot)\n" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "probably max input voltage, we can also look at temperature and " + ] + }, + { + "cell_type": "code", + "execution_count": 14, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Acceleration Read: [20 20]\n" + ] + } + ], + "source": [ + "\n", + "print(\"Acceleration Read:\", robot.arm_bus.read(\"Acceleration\"))" + ] + }, + { + "cell_type": "code", + "execution_count": 37, + "metadata": {}, + "outputs": [], + "source": [ + "robot.arm_bus.write(\"LED_Alarm_Condition\", 2, [\"elbow_flex\"])" + ] + }, + { + "cell_type": "code", + "execution_count": 16, + "metadata": {}, + "outputs": [], + "source": [ + "robot.arm_bus.write(\"Acceleration\", 20, [\"elbow_flex\"])\n", + "robot.arm_bus.write(\"Acceleration\", 100, [\"wrist_yaw\"])" + ] + }, + { + "cell_type": "code", + "execution_count": 73, + "metadata": {}, + "outputs": [], + "source": [] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "robot.arm_bus.write(\"Goal_Position\", [1000, 1000], [\"elbow_flex\", \"wrist_yaw\"])\n", + "time.sleep(2)\n", + "robot.arm_bus.write(\"Goal_Position\", [2000, 2000], [\"elbow_flex\", \"wrist_yaw\"])" + ] + }, + { + "cell_type": "code", + "execution_count": 68, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Elbow Flex Current: [1]\n", + "Elbow Flex Current: [0]\n", + "Elbow Flex Current: [3]\n", + "Elbow Flex Current: [1]\n", + "Elbow Flex Current: [1]\n", + "Elbow Flex Current: [2]\n", + "Elbow Flex Current: [1]\n", + "Elbow Flex Current: [1]\n", + "Elbow Flex Current: [2]\n", + "Elbow Flex Current: [1]\n", + "Elbow Flex Current: [1]\n", + "Elbow Flex Current: [0]\n" + ] + }, + { + "ename": "KeyboardInterrupt", + "evalue": "", + "output_type": "error", + "traceback": [ + "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[0;31mKeyboardInterrupt\u001b[0m Traceback (most recent call last)", + "Cell \u001b[0;32mIn[68], line 25\u001b[0m\n\u001b[1;32m 22\u001b[0m time\u001b[38;5;241m.\u001b[39msleep(\u001b[38;5;241m2\u001b[39m)\n\u001b[1;32m 23\u001b[0m \u001b[38;5;28;01melse\u001b[39;00m:\n\u001b[1;32m 24\u001b[0m \u001b[38;5;66;03m# If current is zero, hold at pos_a for a bit\u001b[39;00m\n\u001b[0;32m---> 25\u001b[0m \u001b[43mtime\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43msleep\u001b[49m\u001b[43m(\u001b[49m\u001b[38;5;241;43m1\u001b[39;49m\u001b[43m)\u001b[49m\n", + "\u001b[0;31mKeyboardInterrupt\u001b[0m: " + ] + } + ], + "source": [ + "import time\n", + "\n", + "# Enable torque on elbow_flex\n", + "robot.arm_bus.write(\"Torque_Enable\", 1, [\"elbow_flex\"])\n", + "\n", + "pos_a = 2500\n", + "pos_b = 1000\n", + "\n", + "robot.arm_bus.write(\"Goal_Position\", pos_a, [\"elbow_flex\"])\n", + "time.sleep(2)\n", + "\n", + "while True:\n", + " current_val = robot.arm_bus.read(\"Present_Current\", \"elbow_flex\")\n", + " print(\"Elbow Flex Current:\", current_val)\n", + " \n", + " # If the servo is under non-zero load/current, switch position\n", + " if current_val > 1:\n", + " robot.arm_bus.write(\"Goal_Position\", pos_b, [\"elbow_flex\"])\n", + " time.sleep(2)\n", + " # Go back to pos_a again\n", + " robot.arm_bus.write(\"Goal_Position\", pos_a, [\"elbow_flex\"])\n", + " time.sleep(2)\n", + " else:\n", + " # If current is zero, hold at pos_a for a bit\n", + " time.sleep(1)\n" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\"Acceleration\" = 0, infinitely fast\n", + "\"Acceleration\" = 20 slow\n", + "elbow_flex is the LED one (4)\n", + "\n", + "robot.arm_bus.write(\"LED_Alarm_Condition\", 2, [\"elbow_flex\"]) #on constantly\n", + "robot.arm_bus.write(\"LED_Alarm_Condition\", 1, [\"elbow_flex\"]) #beeping\n", + "robot.arm_bus.write(\"LED_Alarm_Condition\", 0, [\"elbow_flex\"]) #off\n", + "\n", + "\"Max_Torque_Limit\": (16, 2), is what i have o play around with or \"Protective_Torque\": (37, 1), maybe\n", + "\n", + "robot.arm_bus.write(\"Torque_Enable\", 1, [\"elbow_flex\"]) 1 can move 0 cant move\n", + "\n", + "robot.arm_bus.write(\"Torque_Limit\", 300, [\"elbow_flex\"]) #how strong/weak the servo is. 0 makes it so that it cannot move basically, but i'd like to have that value change honestly and for it to be waeaker\n", + "\n", + "robot.arm_bus.write(\"Torque_Limit\", 20,[\"elbow_flex\"]) 20 in ordre to get some motion\n", + "\n", + "default is 1000\n", + "\n", + "robot.arm_bus.write(\"Goal_Speed\", -s, [\"elbow_flex\"]) #changes how fast the servo moves when going to the target, does not make it move with a specific speed " + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "import time\n", + "\n", + "# Enable torque on elbow_flex\n", + "robot.arm_bus.write(\"Torque_Enable\", 1, [\"elbow_flex\"])\n", + "\n", + "pos_a = 1000\n", + "pos_b = 2500\n", + "\n", + "robot.arm_bus.write(\"Goal_Position\", pos_a, [\"elbow_flex\"])\n", + "time.sleep(2)\n", + "\n", + "while True:\n", + " current_val = robot.arm_bus.read(\"Present_Current\", \"elbow_flex\")\n", + " print(\"Elbow Flex Current:\", current_val)\n", + " \n", + " # If the servo is under non-zero load/current, switch position\n", + " if current_val > 1:\n", + " robot.arm_bus.write(\"Goal_Position\", pos_b, [\"elbow_flex\"])\n", + " time.sleep(2)\n", + " # Go back to pos_a again\n", + " robot.arm_bus.write(\"Goal_Position\", pos_a, [\"elbow_flex\"])\n", + " time.sleep(2)\n", + " else:\n", + " # If current is zero, hold at pos_a for a bit\n", + " time.sleep(1)\n", + "\n", + "\n", + "so if current is larger than x then you disable it \n", + "\n" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [], + "source": [ + "robot.arm_bus.write(\"LED_Alarm_Condition\", 2, [\"elbow_flex\"])" + ] + }, + { + "cell_type": "code", + "execution_count": 43, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[0]\n", + "[0]\n", + "[2]\n", + "[4]\n", + "[0]\n", + "[0]\n", + "[0]\n" + ] + }, + { + "ename": "KeyboardInterrupt", + "evalue": "", + "output_type": "error", + "traceback": [ + "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[0;31mKeyboardInterrupt\u001b[0m Traceback (most recent call last)", + "Cell \u001b[0;32mIn[43], line 3\u001b[0m\n\u001b[1;32m 1\u001b[0m \u001b[38;5;28;01mwhile\u001b[39;00m \u001b[38;5;28;01mTrue\u001b[39;00m:\n\u001b[1;32m 2\u001b[0m \u001b[38;5;28mprint\u001b[39m(robot\u001b[38;5;241m.\u001b[39marm_bus\u001b[38;5;241m.\u001b[39mread(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mPresent_Current\u001b[39m\u001b[38;5;124m\"\u001b[39m, [\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124melbow_flex\u001b[39m\u001b[38;5;124m\"\u001b[39m]))\n\u001b[0;32m----> 3\u001b[0m \u001b[43mtime\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43msleep\u001b[49m\u001b[43m(\u001b[49m\u001b[38;5;241;43m1\u001b[39;49m\u001b[43m)\u001b[49m\n", + "\u001b[0;31mKeyboardInterrupt\u001b[0m: " + ] + } + ], + "source": [ + "while True:\n", + " print(robot.arm_bus.read(\"Present_Current\", [\"elbow_flex\"]))\n", + " time.sleep(1)" + ] + }, + { + "cell_type": "code", + "execution_count": 47, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Max_Voltage_Limit = [160 140 140 160 140 140 80]\n", + "Min_Angle_Limit = [ 650 1300 1300 1200 600 1725 0]\n", + "Max_Angle_Limit = [2600 2050 2800 2500 4096 2250 4095]\n", + "Max_Temperature_Limit = [80 70 70 80 70 70 70]\n", + "Acceleration = [20 20 20 20 20 20 20]\n", + "Torque_Limit = [1000 1000 1000 200 1000 1000 1000]\n", + "Minimum_Startup_Force = [15 16 16 12 16 16 16]\n", + "Protection_Current = [310 310 310 310 310 310 500]\n" + ] + } + ], + "source": [ + "import time\n", + "\n", + "def print_important_params(robot):\n", + "\n", + " # Example parameters you mentioned; adjust as needed\n", + " param_list = [\n", + " \"Max_Voltage_Limit\",\n", + " \"Min_Angle_Limit\",\n", + " \"Max_Angle_Limit\",\n", + " \"Max_Temperature_Limit\",\n", + " \"Acceleration\", # or \"Maximum_Acceleration\" if you prefer that register\n", + " \"Torque_Limit\", # or \"Max_Torque_Limit\" if your table uses that\n", + " \"Minimum_Startup_Force\",\n", + " \"Protection_Current\",\n", + " ]\n", + " \n", + " for param in param_list:\n", + " try:\n", + " val = robot.arm_bus.read(param)\n", + " print(f\"{param} = {val}\")\n", + " except Exception as e:\n", + " print(f\"{param} read failed: {e}\")\n", + "\n", + "# -------------------------------\n", + "# Example usage\n", + "\n", + "print_important_params(robot)\n" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "lerobot", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.10.16" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/examples/test_torque/test_torque.py b/examples/test_torque/test_torque.py new file mode 100644 index 00000000..5936d90c --- /dev/null +++ b/examples/test_torque/test_torque.py @@ -0,0 +1,27 @@ +import time +from hopejr import HopeJuniorRobot + + +def main(): + # Instantiate and connect to the robot + robot = HopeJuniorRobot() + robot.connect() + + # Example read of the current position + print("Present Position:", robot.arm_bus.read("Present_Position")) + + # Enable torque and set acceleration + robot.arm_bus.write("Torque_Enable", 1) + robot.arm_bus.write("Acceleration", 20) + print("Acceleration Read:", robot.arm_bus.read("Acceleration")) + + # Move elbow_flex and wrist_yaw a few times + robot.arm_bus.write("Goal_Position", [1000, 1000], ["elbow_flex", "wrist_yaw"]) + time.sleep(2) + robot.arm_bus.write("Goal_Position", [1500, 1500], ["elbow_flex", "wrist_yaw"]) + time.sleep(2) + robot.arm_bus.write("Goal_Position", [1000, 1000], ["elbow_flex", "wrist_yaw"]) + + +if __name__ == "__main__": + main() diff --git a/examples/test_torque/var_table.json b/examples/test_torque/var_table.json new file mode 100644 index 00000000..0bb06b18 --- /dev/null +++ b/examples/test_torque/var_table.json @@ -0,0 +1,49 @@ +STS_SERIES_CONTROL_TABLE = { + "Model": (3, 2), + "ID": (5, 1), + "Baud_Rate": (6, 1), + "Return_Delay": (7, 1), + "Response_Status_Level": (8, 1), + "Min_Angle_Limit": (9, 2), + "Max_Angle_Limit": (11, 2), + "Max_Temperature_Limit": (13, 1), + "Max_Voltage_Limit": (14, 1), + "Min_Voltage_Limit": (15, 1), + "Max_Torque_Limit": (16, 2), + "Phase": (18, 1), + "Unloading_Condition": (19, 1), + "LED_Alarm_Condition": (20, 1), + "P_Coefficient": (21, 1), + "D_Coefficient": (22, 1), + "I_Coefficient": (23, 1), + "Minimum_Startup_Force": (24, 2), + "CW_Dead_Zone": (26, 1), + "CCW_Dead_Zone": (27, 1), + "Protection_Current": (28, 2), + "Angular_Resolution": (30, 1), + "Offset": (31, 2), + "Mode": (33, 1), + "Protective_Torque": (34, 1), + "Protection_Time": (35, 1), + "Overload_Torque": (36, 1), + "Speed_closed_loop_P_proportional_coefficient": (37, 1), + "Over_Current_Protection_Time": (38, 1), + "Velocity_closed_loop_I_integral_coefficient": (39, 1), + "Torque_Enable": (40, 1), + "Acceleration": (41, 1), + "Goal_Position": (42, 2), + "Goal_Time": (44, 2), + "Goal_Speed": (46, 2), + "Torque_Limit": (48, 2), + "Lock": (55, 1), + "Present_Position": (56, 2), + "Present_Speed": (58, 2), + "Present_Load": (60, 2), + "Present_Voltage": (62, 1), + "Present_Temperature": (63, 1), + "Status": (65, 1), + "Moving": (66, 1), + "Present_Current": (69, 2), + # Not in the Memory Table + "Maximum_Acceleration": (85, 2), +} \ No newline at end of file diff --git a/examples/test_torque/why it fails b/examples/test_torque/why it fails new file mode 100644 index 00000000..82d7332b --- /dev/null +++ b/examples/test_torque/why it fails @@ -0,0 +1,16 @@ + +First check that kicks in is current: +Protection_Current (310) amperes or sth +Present_Current, compared against protection crrent +Over_Current_Protection_Time, how long until you shut it down + +make a quick update about this + +variables of interest are +Max_Torque_Limit = 1000, +Present_Load = 1000-something, which triggered the overload torque mechanism +Overload_Torque = 80, how much of the max torque limit do we allow? +Protection_Time = 200, after how long do we set Torque_Enable to 1? *not true lol +Protective_Torque = 20, after we trigger the safety mechanism, how much torque do we allow the motor to have? + +theres actually no temperature or voltage check that the feetechs perform, the only two are current and torque, which works like i said above diff --git a/nice were not even close .png b/nice were not even close .png new file mode 100644 index 00000000..9a43f6b5 Binary files /dev/null and b/nice were not even close .png differ diff --git a/servo_log.txt b/servo_log.txt new file mode 100644 index 00000000..db711b60 --- /dev/null +++ b/servo_log.txt @@ -0,0 +1,174 @@ +Logging parameters for servo: shoulder_pitch +Duration: 2s, Interval: 0.1s +================================================== +Timestamp: 0.000 +Present_Current = [19] +Protection_Current = [310] +Over_Current_Protection_Time = [50] +Present_Temperature = [37] +Present_Voltage = [123] +Present_Load = [123] +Present_Position = [1803] +-------------------------------------------------- +Timestamp: 0.105 +Present_Current = [51] +Protection_Current = [310] +Over_Current_Protection_Time = [50] +Present_Temperature = [37] +Present_Voltage = [121] +Present_Load = [207] +Present_Position = [1742] +-------------------------------------------------- +Timestamp: 0.211 +Present_Current = [130] +Protection_Current = [310] +Over_Current_Protection_Time = [50] +Present_Temperature = [37] +Present_Voltage = [120] +Present_Load = [363] +Present_Position = [1668] +-------------------------------------------------- +Timestamp: 0.316 +Present_Current = [192] +Protection_Current = [310] +Over_Current_Protection_Time = [50] +Present_Temperature = [37] +Present_Voltage = [120] +Present_Load = [471] +Present_Position = [1564] +-------------------------------------------------- +Timestamp: 0.422 +Present_Current = [177] +Protection_Current = [310] +Over_Current_Protection_Time = [50] +Present_Temperature = [37] +Present_Voltage = [120] +Present_Load = [503] +Present_Position = [1428] +-------------------------------------------------- +Timestamp: 0.528 +Present_Current = [294] +Protection_Current = [310] +Over_Current_Protection_Time = [50] +Present_Temperature = [37] +Present_Voltage = [118] +Present_Load = [615] +Present_Position = [1282] +-------------------------------------------------- +Timestamp: 0.633 +Present_Current = [360] +Protection_Current = [310] +Over_Current_Protection_Time = [50] +Present_Temperature = [37] +Present_Voltage = [118] +Present_Load = [671] +Present_Position = [1141] +-------------------------------------------------- +Timestamp: 0.739 +Present_Current = [360] +Protection_Current = [310] +Over_Current_Protection_Time = [50] +Present_Temperature = [37] +Present_Voltage = [118] +Present_Load = [639] +Present_Position = [1012] +-------------------------------------------------- +Timestamp: 0.845 +Present_Current = [348] +Protection_Current = [310] +Over_Current_Protection_Time = [50] +Present_Temperature = [37] +Present_Voltage = [118] +Present_Load = [615] +Present_Position = [907] +-------------------------------------------------- +Timestamp: 0.951 +Present_Current = [407] +Protection_Current = [310] +Over_Current_Protection_Time = [50] +Present_Temperature = [37] +Present_Voltage = [118] +Present_Load = [643] +Present_Position = [830] +-------------------------------------------------- +Timestamp: 1.057 +Present_Current = [479] +Protection_Current = [310] +Over_Current_Protection_Time = [50] +Present_Temperature = [38] +Present_Voltage = [117] +Present_Load = [679] +Present_Position = [776] +-------------------------------------------------- +Timestamp: 1.163 +Present_Current = [0] +Protection_Current = [310] +Over_Current_Protection_Time = [50] +Present_Temperature = [37] +Present_Voltage = [122] +Present_Load = [0] +Present_Position = [947] +-------------------------------------------------- +Timestamp: 1.269 +Present_Current = [0] +Protection_Current = [310] +Over_Current_Protection_Time = [50] +Present_Temperature = [37] +Present_Voltage = [122] +Present_Load = [0] +Present_Position = [1136] +-------------------------------------------------- +Timestamp: 1.375 +Present_Current = [0] +Protection_Current = [310] +Over_Current_Protection_Time = [50] +Present_Temperature = [37] +Present_Voltage = [122] +Present_Load = [0] +Present_Position = [1438] +-------------------------------------------------- +Timestamp: 1.481 +Present_Current = [0] +Protection_Current = [310] +Over_Current_Protection_Time = [50] +Present_Temperature = [37] +Present_Voltage = [122] +Present_Load = [0] +Present_Position = [1782] +-------------------------------------------------- +Timestamp: 1.587 +Present_Current = [0] +Protection_Current = [310] +Over_Current_Protection_Time = [50] +Present_Temperature = [37] +Present_Voltage = [122] +Present_Load = [0] +Present_Position = [1949] +-------------------------------------------------- +Timestamp: 1.693 +Present_Current = [0] +Protection_Current = [310] +Over_Current_Protection_Time = [50] +Present_Temperature = [37] +Present_Voltage = [122] +Present_Load = [0] +Present_Position = [2042] +-------------------------------------------------- +Timestamp: 1.799 +Present_Current = [0] +Protection_Current = [310] +Over_Current_Protection_Time = [50] +Present_Temperature = [37] +Present_Voltage = [122] +Present_Load = [0] +Present_Position = [2049] +-------------------------------------------------- +Timestamp: 1.906 +Present_Current = [0] +Protection_Current = [310] +Over_Current_Protection_Time = [50] +Present_Temperature = [37] +Present_Voltage = [122] +Present_Load = [0] +Present_Position = [1997] +-------------------------------------------------- diff --git a/shoulder_pitch_fail.txt b/shoulder_pitch_fail.txt new file mode 100644 index 00000000..345da89c --- /dev/null +++ b/shoulder_pitch_fail.txt @@ -0,0 +1,1107 @@ +Logging STS servo parameters for servo: shoulder_pitch +Interval: 0.1 seconds +================================================== +Timestamp: 1736435663.227 +Model = [2825] +ID = [1] +Baud_Rate = [0] +Return_Delay = [0] +Response_Status_Level = [1] +Min_Angle_Limit = [650] +Max_Angle_Limit = [2600] +Max_Temperature_Limit = [80] +Max_Voltage_Limit = [160] +Min_Voltage_Limit = [60] +Max_Torque_Limit = [1000] +Phase = [12] +Unloading_Condition = [45] +LED_Alarm_Condition = [45] +P_Coefficient = [32] +D_Coefficient = [32] +I_Coefficient = [0] +Minimum_Startup_Force = [15] +CW_Dead_Zone = [0] +CCW_Dead_Zone = [0] +Protection_Current = [310] +Angular_Resolution = [1] +Offset = [0] +Mode = [0] +Protective_Torque = [20] +Protection_Time = [200] +Overload_Torque = [80] +Speed_closed_loop_P_proportional_coefficient = [10] +Over_Current_Protection_Time = [50] +Velocity_closed_loop_I_integral_coefficient = [200] +Torque_Enable = [1] +Acceleration = [20] +Goal_Position = [600] +Goal_Time = [0] +Goal_Speed = [0] +Torque_Limit = [1000] +Lock = [1] +Present_Position = [1862] +Present_Speed = [0] +Present_Load = [31] +Present_Voltage = [122] +Present_Temperature = [44] +Status = [0] +Moving = [1] +Present_Current = [2] +Maximum_Acceleration = [1530] +-------------------------------------------------- +Timestamp: 1736435663.357 +Model = [2825] +ID = [1] +Baud_Rate = [0] +Return_Delay = [0] +Response_Status_Level = [1] +Min_Angle_Limit = [650] +Max_Angle_Limit = [2600] +Max_Temperature_Limit = [80] +Max_Voltage_Limit = [160] +Min_Voltage_Limit = [60] +Max_Torque_Limit = [1000] +Phase = [12] +Unloading_Condition = [45] +LED_Alarm_Condition = [45] +P_Coefficient = [32] +D_Coefficient = [32] +I_Coefficient = [0] +Minimum_Startup_Force = [15] +CW_Dead_Zone = [0] +CCW_Dead_Zone = [0] +Protection_Current = [310] +Angular_Resolution = [1] +Offset = [0] +Mode = [0] +Protective_Torque = [20] +Protection_Time = [200] +Overload_Torque = [80] +Speed_closed_loop_P_proportional_coefficient = [10] +Over_Current_Protection_Time = [50] +Velocity_closed_loop_I_integral_coefficient = [200] +Torque_Enable = [1] +Acceleration = [20] +Goal_Position = [600] +Goal_Time = [0] +Goal_Speed = [0] +Torque_Limit = [1000] +Lock = [1] +Present_Position = [1837] +Present_Speed = [33068] +Present_Load = [59] +Present_Voltage = [122] +Present_Temperature = [44] +Status = [0] +Moving = [1] +Present_Current = [5] +Maximum_Acceleration = [1530] +-------------------------------------------------- +Timestamp: 1736435663.488 +Model = [2825] +ID = [1] +Baud_Rate = [0] +Return_Delay = [0] +Response_Status_Level = [1] +Min_Angle_Limit = [650] +Max_Angle_Limit = [2600] +Max_Temperature_Limit = [80] +Max_Voltage_Limit = [160] +Min_Voltage_Limit = [60] +Max_Torque_Limit = [1000] +Phase = [12] +Unloading_Condition = [45] +LED_Alarm_Condition = [45] +P_Coefficient = [32] +D_Coefficient = [32] +I_Coefficient = [0] +Minimum_Startup_Force = [15] +CW_Dead_Zone = [0] +CCW_Dead_Zone = [0] +Protection_Current = [310] +Angular_Resolution = [1] +Offset = [0] +Mode = [0] +Protective_Torque = [20] +Protection_Time = [200] +Overload_Torque = [80] +Speed_closed_loop_P_proportional_coefficient = [10] +Over_Current_Protection_Time = [50] +Velocity_closed_loop_I_integral_coefficient = [200] +Torque_Enable = [1] +Acceleration = [20] +Goal_Position = [600] +Goal_Time = [0] +Goal_Speed = [0] +Torque_Limit = [1000] +Lock = [1] +Present_Position = [1780] +Present_Speed = [33368] +Present_Load = [147] +Present_Voltage = [125] +Present_Temperature = [44] +Status = [0] +Moving = [1] +Present_Current = [32] +Maximum_Acceleration = [1530] +-------------------------------------------------- +Timestamp: 1736435663.621 +Model = [2825] +ID = [1] +Baud_Rate = [0] +Return_Delay = [0] +Response_Status_Level = [1] +Min_Angle_Limit = [650] +Max_Angle_Limit = [2600] +Max_Temperature_Limit = [80] +Max_Voltage_Limit = [160] +Min_Voltage_Limit = [60] +Max_Torque_Limit = [1000] +Phase = [12] +Unloading_Condition = [45] +LED_Alarm_Condition = [45] +P_Coefficient = [32] +D_Coefficient = [32] +I_Coefficient = [0] +Minimum_Startup_Force = [15] +CW_Dead_Zone = [0] +CCW_Dead_Zone = [0] +Protection_Current = [310] +Angular_Resolution = [1] +Offset = [0] +Mode = [0] +Protective_Torque = [20] +Protection_Time = [200] +Overload_Torque = [80] +Speed_closed_loop_P_proportional_coefficient = [10] +Over_Current_Protection_Time = [50] +Velocity_closed_loop_I_integral_coefficient = [200] +Torque_Enable = [1] +Acceleration = [20] +Goal_Position = [600] +Goal_Time = [0] +Goal_Speed = [0] +Torque_Limit = [1000] +Lock = [1] +Present_Position = [1699] +Present_Speed = [33418] +Present_Load = [295] +Present_Voltage = [120] +Present_Temperature = [44] +Status = [0] +Moving = [1] +Present_Current = [97] +Maximum_Acceleration = [1530] +-------------------------------------------------- +Timestamp: 1736435663.753 +Model = [2825] +ID = [1] +Baud_Rate = [0] +Return_Delay = [0] +Response_Status_Level = [1] +Min_Angle_Limit = [650] +Max_Angle_Limit = [2600] +Max_Temperature_Limit = [80] +Max_Voltage_Limit = [160] +Min_Voltage_Limit = [60] +Max_Torque_Limit = [1000] +Phase = [12] +Unloading_Condition = [45] +LED_Alarm_Condition = [45] +P_Coefficient = [32] +D_Coefficient = [32] +I_Coefficient = [0] +Minimum_Startup_Force = [15] +CW_Dead_Zone = [0] +CCW_Dead_Zone = [0] +Protection_Current = [310] +Angular_Resolution = [1] +Offset = [0] +Mode = [0] +Protective_Torque = [20] +Protection_Time = [200] +Overload_Torque = [80] +Speed_closed_loop_P_proportional_coefficient = [10] +Over_Current_Protection_Time = [50] +Velocity_closed_loop_I_integral_coefficient = [200] +Torque_Enable = [1] +Acceleration = [20] +Goal_Position = [600] +Goal_Time = [0] +Goal_Speed = [0] +Torque_Limit = [1000] +Lock = [1] +Present_Position = [1583] +Present_Speed = [33818] +Present_Load = [463] +Present_Voltage = [119] +Present_Temperature = [44] +Status = [0] +Moving = [1] +Present_Current = [182] +Maximum_Acceleration = [1530] +-------------------------------------------------- +Timestamp: 1736435663.884 +Model = [2825] +ID = [1] +Baud_Rate = [0] +Return_Delay = [0] +Response_Status_Level = [1] +Min_Angle_Limit = [650] +Max_Angle_Limit = [2600] +Max_Temperature_Limit = [80] +Max_Voltage_Limit = [160] +Min_Voltage_Limit = [60] +Max_Torque_Limit = [1000] +Phase = [12] +Unloading_Condition = [45] +LED_Alarm_Condition = [45] +P_Coefficient = [32] +D_Coefficient = [32] +I_Coefficient = [0] +Minimum_Startup_Force = [15] +CW_Dead_Zone = [0] +CCW_Dead_Zone = [0] +Protection_Current = [310] +Angular_Resolution = [1] +Offset = [0] +Mode = [0] +Protective_Torque = [20] +Protection_Time = [200] +Overload_Torque = [80] +Speed_closed_loop_P_proportional_coefficient = [10] +Over_Current_Protection_Time = [50] +Velocity_closed_loop_I_integral_coefficient = [200] +Torque_Enable = [1] +Acceleration = [20] +Goal_Position = [600] +Goal_Time = [0] +Goal_Speed = [0] +Torque_Limit = [1000] +Lock = [1] +Present_Position = [1418] +Present_Speed = [34068] +Present_Load = [503] +Present_Voltage = [120] +Present_Temperature = [44] +Status = [0] +Moving = [1] +Present_Current = [163] +Maximum_Acceleration = [1530] +-------------------------------------------------- +Timestamp: 1736435664.016 +Model = [2825] +ID = [1] +Baud_Rate = [0] +Return_Delay = [0] +Response_Status_Level = [1] +Min_Angle_Limit = [650] +Max_Angle_Limit = [2600] +Max_Temperature_Limit = [80] +Max_Voltage_Limit = [160] +Min_Voltage_Limit = [60] +Max_Torque_Limit = [1000] +Phase = [12] +Unloading_Condition = [45] +LED_Alarm_Condition = [45] +P_Coefficient = [32] +D_Coefficient = [32] +I_Coefficient = [0] +Minimum_Startup_Force = [15] +CW_Dead_Zone = [0] +CCW_Dead_Zone = [0] +Protection_Current = [310] +Angular_Resolution = [1] +Offset = [0] +Mode = [0] +Protective_Torque = [20] +Protection_Time = [200] +Overload_Torque = [80] +Speed_closed_loop_P_proportional_coefficient = [10] +Over_Current_Protection_Time = [50] +Velocity_closed_loop_I_integral_coefficient = [200] +Torque_Enable = [1] +Acceleration = [20] +Goal_Position = [600] +Goal_Time = [0] +Goal_Speed = [0] +Torque_Limit = [1000] +Lock = [1] +Present_Position = [1240] +Present_Speed = [34068] +Present_Load = [659] +Present_Voltage = [119] +Present_Temperature = [44] +Status = [0] +Moving = [1] +Present_Current = [313] +Maximum_Acceleration = [1530] +-------------------------------------------------- +Timestamp: 1736435664.148 +Model = [2825] +ID = [1] +Baud_Rate = [0] +Return_Delay = [0] +Response_Status_Level = [1] +Min_Angle_Limit = [650] +Max_Angle_Limit = [2600] +Max_Temperature_Limit = [80] +Max_Voltage_Limit = [160] +Min_Voltage_Limit = [60] +Max_Torque_Limit = [1000] +Phase = [12] +Unloading_Condition = [45] +LED_Alarm_Condition = [45] +P_Coefficient = [32] +D_Coefficient = [32] +I_Coefficient = [0] +Minimum_Startup_Force = [15] +CW_Dead_Zone = [0] +CCW_Dead_Zone = [0] +Protection_Current = [310] +Angular_Resolution = [1] +Offset = [0] +Mode = [0] +Protective_Torque = [20] +Protection_Time = [200] +Overload_Torque = [80] +Speed_closed_loop_P_proportional_coefficient = [10] +Over_Current_Protection_Time = [50] +Velocity_closed_loop_I_integral_coefficient = [200] +Torque_Enable = [1] +Acceleration = [20] +Goal_Position = [600] +Goal_Time = [0] +Goal_Speed = [0] +Torque_Limit = [1000] +Lock = [1] +Present_Position = [1071] +Present_Speed = [33968] +Present_Load = [667] +Present_Voltage = [118] +Present_Temperature = [44] +Status = [0] +Moving = [1] +Present_Current = [384] +Maximum_Acceleration = [1530] +-------------------------------------------------- +Timestamp: 1736435664.279 +Model = [2825] +ID = [1] +Baud_Rate = [0] +Return_Delay = [0] +Response_Status_Level = [1] +Min_Angle_Limit = [650] +Max_Angle_Limit = [2600] +Max_Temperature_Limit = [80] +Max_Voltage_Limit = [160] +Min_Voltage_Limit = [60] +Max_Torque_Limit = [1000] +Phase = [12] +Unloading_Condition = [45] +LED_Alarm_Condition = [45] +P_Coefficient = [32] +D_Coefficient = [32] +I_Coefficient = [0] +Minimum_Startup_Force = [15] +CW_Dead_Zone = [0] +CCW_Dead_Zone = [0] +Protection_Current = [310] +Angular_Resolution = [1] +Offset = [0] +Mode = [0] +Protective_Torque = [20] +Protection_Time = [200] +Overload_Torque = [80] +Speed_closed_loop_P_proportional_coefficient = [10] +Over_Current_Protection_Time = [50] +Velocity_closed_loop_I_integral_coefficient = [200] +Torque_Enable = [1] +Acceleration = [20] +Goal_Position = [600] +Goal_Time = [0] +Goal_Speed = [0] +Torque_Limit = [1000] +Lock = [1] +Present_Position = [932] +Present_Speed = [33718] +Present_Load = [655] +Present_Voltage = [118] +Present_Temperature = [44] +Status = [0] +Moving = [1] +Present_Current = [374] +Maximum_Acceleration = [1530] +-------------------------------------------------- +Timestamp: 1736435664.411 +Model = [2825] +ID = [1] +Baud_Rate = [0] +Return_Delay = [0] +Response_Status_Level = [1] +Min_Angle_Limit = [650] +Max_Angle_Limit = [2600] +Max_Temperature_Limit = [80] +Max_Voltage_Limit = [160] +Min_Voltage_Limit = [60] +Max_Torque_Limit = [1000] +Phase = [12] +Unloading_Condition = [45] +LED_Alarm_Condition = [45] +P_Coefficient = [32] +D_Coefficient = [32] +I_Coefficient = [0] +Minimum_Startup_Force = [15] +CW_Dead_Zone = [0] +CCW_Dead_Zone = [0] +Protection_Current = [310] +Angular_Resolution = [1] +Offset = [0] +Mode = [0] +Protective_Torque = [20] +Protection_Time = [200] +Overload_Torque = [80] +Speed_closed_loop_P_proportional_coefficient = [10] +Over_Current_Protection_Time = [50] +Velocity_closed_loop_I_integral_coefficient = [200] +Torque_Enable = [1] +Acceleration = [20] +Goal_Position = [600] +Goal_Time = [0] +Goal_Speed = [0] +Torque_Limit = [1000] +Lock = [1] +Present_Position = [833] +Present_Speed = [33468] +Present_Load = [675] +Present_Voltage = [118] +Present_Temperature = [44] +Status = [0] +Moving = [1] +Present_Current = [443] +Maximum_Acceleration = [1530] +-------------------------------------------------- +Timestamp: 1736435664.542 +Model = [2825] +ID = [1] +Baud_Rate = [0] +Return_Delay = [0] +Response_Status_Level = [1] +Min_Angle_Limit = [650] +Max_Angle_Limit = [2600] +Max_Temperature_Limit = [80] +Max_Voltage_Limit = [160] +Min_Voltage_Limit = [60] +Max_Torque_Limit = [1000] +Phase = [12] +Unloading_Condition = [45] +LED_Alarm_Condition = [45] +P_Coefficient = [32] +D_Coefficient = [32] +I_Coefficient = [0] +Minimum_Startup_Force = [15] +CW_Dead_Zone = [0] +CCW_Dead_Zone = [0] +Protection_Current = [310] +Angular_Resolution = [1] +Offset = [0] +Mode = [0] +Protective_Torque = [20] +Protection_Time = [200] +Overload_Torque = [80] +Speed_closed_loop_P_proportional_coefficient = [10] +Over_Current_Protection_Time = [50] +Velocity_closed_loop_I_integral_coefficient = [200] +Torque_Enable = [0] +Acceleration = [20] +Goal_Position = [600] +Goal_Time = [0] +Goal_Speed = [0] +Torque_Limit = [1000] +Lock = [1] +Present_Position = [784] +Present_Speed = [150] +Present_Load = [0] +Present_Voltage = [122] +Present_Temperature = [44] +Status = [8] +Moving = [1] +Present_Current = [0] +Maximum_Acceleration = [1530] +-------------------------------------------------- +Timestamp: 1736435664.674 +Model = [2825] +ID = [1] +Baud_Rate = [0] +Return_Delay = [0] +Response_Status_Level = [1] +Min_Angle_Limit = [650] +Max_Angle_Limit = [2600] +Max_Temperature_Limit = [80] +Max_Voltage_Limit = [160] +Min_Voltage_Limit = [60] +Max_Torque_Limit = [1000] +Phase = [12] +Unloading_Condition = [45] +LED_Alarm_Condition = [45] +P_Coefficient = [32] +D_Coefficient = [32] +I_Coefficient = [0] +Minimum_Startup_Force = [15] +CW_Dead_Zone = [0] +CCW_Dead_Zone = [0] +Protection_Current = [310] +Angular_Resolution = [1] +Offset = [0] +Mode = [0] +Protective_Torque = [20] +Protection_Time = [200] +Overload_Torque = [80] +Speed_closed_loop_P_proportional_coefficient = [10] +Over_Current_Protection_Time = [50] +Velocity_closed_loop_I_integral_coefficient = [200] +Torque_Enable = [0] +Acceleration = [20] +Goal_Position = [600] +Goal_Time = [0] +Goal_Speed = [0] +Torque_Limit = [1000] +Lock = [1] +Present_Position = [1052] +Present_Speed = [2000] +Present_Load = [0] +Present_Voltage = [123] +Present_Temperature = [44] +Status = [8] +Moving = [1] +Present_Current = [0] +Maximum_Acceleration = [1530] +-------------------------------------------------- +Timestamp: 1736435664.806 +Model = [2825] +ID = [1] +Baud_Rate = [0] +Return_Delay = [0] +Response_Status_Level = [1] +Min_Angle_Limit = [650] +Max_Angle_Limit = [2600] +Max_Temperature_Limit = [80] +Max_Voltage_Limit = [160] +Min_Voltage_Limit = [60] +Max_Torque_Limit = [1000] +Phase = [12] +Unloading_Condition = [45] +LED_Alarm_Condition = [45] +P_Coefficient = [32] +D_Coefficient = [32] +I_Coefficient = [0] +Minimum_Startup_Force = [15] +CW_Dead_Zone = [0] +CCW_Dead_Zone = [0] +Protection_Current = [310] +Angular_Resolution = [1] +Offset = [0] +Mode = [0] +Protective_Torque = [20] +Protection_Time = [200] +Overload_Torque = [80] +Speed_closed_loop_P_proportional_coefficient = [10] +Over_Current_Protection_Time = [50] +Velocity_closed_loop_I_integral_coefficient = [200] +Torque_Enable = [0] +Acceleration = [20] +Goal_Position = [600] +Goal_Time = [0] +Goal_Speed = [0] +Torque_Limit = [1000] +Lock = [1] +Present_Position = [1274] +Present_Speed = [1550] +Present_Load = [0] +Present_Voltage = [122] +Present_Temperature = [44] +Status = [8] +Moving = [1] +Present_Current = [0] +Maximum_Acceleration = [1530] +-------------------------------------------------- +Timestamp: 1736435664.937 +Model = [2825] +ID = [1] +Baud_Rate = [0] +Return_Delay = [0] +Response_Status_Level = [1] +Min_Angle_Limit = [650] +Max_Angle_Limit = [2600] +Max_Temperature_Limit = [80] +Max_Voltage_Limit = [160] +Min_Voltage_Limit = [60] +Max_Torque_Limit = [1000] +Phase = [12] +Unloading_Condition = [45] +LED_Alarm_Condition = [45] +P_Coefficient = [32] +D_Coefficient = [32] +I_Coefficient = [0] +Minimum_Startup_Force = [15] +CW_Dead_Zone = [0] +CCW_Dead_Zone = [0] +Protection_Current = [310] +Angular_Resolution = [1] +Offset = [0] +Mode = [0] +Protective_Torque = [20] +Protection_Time = [200] +Overload_Torque = [80] +Speed_closed_loop_P_proportional_coefficient = [10] +Over_Current_Protection_Time = [50] +Velocity_closed_loop_I_integral_coefficient = [200] +Torque_Enable = [0] +Acceleration = [20] +Goal_Position = [600] +Goal_Time = [0] +Goal_Speed = [0] +Torque_Limit = [1000] +Lock = [1] +Present_Position = [1513] +Present_Speed = [2150] +Present_Load = [0] +Present_Voltage = [122] +Present_Temperature = [44] +Status = [8] +Moving = [1] +Present_Current = [0] +Maximum_Acceleration = [1530] +-------------------------------------------------- +Timestamp: 1736435665.069 +Model = [2825] +ID = [1] +Baud_Rate = [0] +Return_Delay = [0] +Response_Status_Level = [1] +Min_Angle_Limit = [650] +Max_Angle_Limit = [2600] +Max_Temperature_Limit = [80] +Max_Voltage_Limit = [160] +Min_Voltage_Limit = [60] +Max_Torque_Limit = [1000] +Phase = [12] +Unloading_Condition = [45] +LED_Alarm_Condition = [45] +P_Coefficient = [32] +D_Coefficient = [32] +I_Coefficient = [0] +Minimum_Startup_Force = [15] +CW_Dead_Zone = [0] +CCW_Dead_Zone = [0] +Protection_Current = [310] +Angular_Resolution = [1] +Offset = [0] +Mode = [0] +Protective_Torque = [20] +Protection_Time = [200] +Overload_Torque = [80] +Speed_closed_loop_P_proportional_coefficient = [10] +Over_Current_Protection_Time = [50] +Velocity_closed_loop_I_integral_coefficient = [200] +Torque_Enable = [0] +Acceleration = [20] +Goal_Position = [600] +Goal_Time = [0] +Goal_Speed = [0] +Torque_Limit = [1000] +Lock = [1] +Present_Position = [1756] +Present_Speed = [1600] +Present_Load = [0] +Present_Voltage = [122] +Present_Temperature = [44] +Status = [8] +Moving = [1] +Present_Current = [0] +Maximum_Acceleration = [1530] +-------------------------------------------------- +Timestamp: 1736435665.200 +Model = [2825] +ID = [1] +Baud_Rate = [0] +Return_Delay = [0] +Response_Status_Level = [1] +Min_Angle_Limit = [650] +Max_Angle_Limit = [2600] +Max_Temperature_Limit = [80] +Max_Voltage_Limit = [160] +Min_Voltage_Limit = [60] +Max_Torque_Limit = [1000] +Phase = [12] +Unloading_Condition = [45] +LED_Alarm_Condition = [45] +P_Coefficient = [32] +D_Coefficient = [32] +I_Coefficient = [0] +Minimum_Startup_Force = [15] +CW_Dead_Zone = [0] +CCW_Dead_Zone = [0] +Protection_Current = [310] +Angular_Resolution = [1] +Offset = [0] +Mode = [0] +Protective_Torque = [20] +Protection_Time = [200] +Overload_Torque = [80] +Speed_closed_loop_P_proportional_coefficient = [10] +Over_Current_Protection_Time = [50] +Velocity_closed_loop_I_integral_coefficient = [200] +Torque_Enable = [0] +Acceleration = [20] +Goal_Position = [600] +Goal_Time = [0] +Goal_Speed = [0] +Torque_Limit = [1000] +Lock = [1] +Present_Position = [1919] +Present_Speed = [750] +Present_Load = [0] +Present_Voltage = [123] +Present_Temperature = [44] +Status = [8] +Moving = [1] +Present_Current = [0] +Maximum_Acceleration = [1530] +-------------------------------------------------- +Timestamp: 1736435665.332 +Model = [2825] +ID = [1] +Baud_Rate = [0] +Return_Delay = [0] +Response_Status_Level = [1] +Min_Angle_Limit = [650] +Max_Angle_Limit = [2600] +Max_Temperature_Limit = [80] +Max_Voltage_Limit = [160] +Min_Voltage_Limit = [60] +Max_Torque_Limit = [1000] +Phase = [12] +Unloading_Condition = [45] +LED_Alarm_Condition = [45] +P_Coefficient = [32] +D_Coefficient = [32] +I_Coefficient = [0] +Minimum_Startup_Force = [15] +CW_Dead_Zone = [0] +CCW_Dead_Zone = [0] +Protection_Current = [310] +Angular_Resolution = [1] +Offset = [0] +Mode = [0] +Protective_Torque = [20] +Protection_Time = [200] +Overload_Torque = [80] +Speed_closed_loop_P_proportional_coefficient = [10] +Over_Current_Protection_Time = [50] +Velocity_closed_loop_I_integral_coefficient = [200] +Torque_Enable = [0] +Acceleration = [20] +Goal_Position = [600] +Goal_Time = [0] +Goal_Speed = [0] +Torque_Limit = [1000] +Lock = [1] +Present_Position = [1937] +Present_Speed = [50] +Present_Load = [0] +Present_Voltage = [122] +Present_Temperature = [44] +Status = [8] +Moving = [0] +Present_Current = [0] +Maximum_Acceleration = [1530] +-------------------------------------------------- +Timestamp: 1736435665.464 +Model = [2825] +ID = [1] +Baud_Rate = [0] +Return_Delay = [0] +Response_Status_Level = [1] +Min_Angle_Limit = [650] +Max_Angle_Limit = [2600] +Max_Temperature_Limit = [80] +Max_Voltage_Limit = [160] +Min_Voltage_Limit = [60] +Max_Torque_Limit = [1000] +Phase = [12] +Unloading_Condition = [45] +LED_Alarm_Condition = [45] +P_Coefficient = [32] +D_Coefficient = [32] +I_Coefficient = [0] +Minimum_Startup_Force = [15] +CW_Dead_Zone = [0] +CCW_Dead_Zone = [0] +Protection_Current = [310] +Angular_Resolution = [1] +Offset = [0] +Mode = [0] +Protective_Torque = [20] +Protection_Time = [200] +Overload_Torque = [80] +Speed_closed_loop_P_proportional_coefficient = [10] +Over_Current_Protection_Time = [50] +Velocity_closed_loop_I_integral_coefficient = [200] +Torque_Enable = [0] +Acceleration = [20] +Goal_Position = [600] +Goal_Time = [0] +Goal_Speed = [0] +Torque_Limit = [1000] +Lock = [1] +Present_Position = [1929] +Present_Speed = [32918] +Present_Load = [0] +Present_Voltage = [122] +Present_Temperature = [44] +Status = [8] +Moving = [0] +Present_Current = [0] +Maximum_Acceleration = [1530] +-------------------------------------------------- +Timestamp: 1736435665.594 +Model = [2825] +ID = [1] +Baud_Rate = [0] +Return_Delay = [0] +Response_Status_Level = [1] +Min_Angle_Limit = [650] +Max_Angle_Limit = [2600] +Max_Temperature_Limit = [80] +Max_Voltage_Limit = [160] +Min_Voltage_Limit = [60] +Max_Torque_Limit = [1000] +Phase = [12] +Unloading_Condition = [45] +LED_Alarm_Condition = [45] +P_Coefficient = [32] +D_Coefficient = [32] +I_Coefficient = [0] +Minimum_Startup_Force = [15] +CW_Dead_Zone = [0] +CCW_Dead_Zone = [0] +Protection_Current = [310] +Angular_Resolution = [1] +Offset = [0] +Mode = [0] +Protective_Torque = [20] +Protection_Time = [200] +Overload_Torque = [80] +Speed_closed_loop_P_proportional_coefficient = [10] +Over_Current_Protection_Time = [50] +Velocity_closed_loop_I_integral_coefficient = [200] +Torque_Enable = [0] +Acceleration = [20] +Goal_Position = [600] +Goal_Time = [0] +Goal_Speed = [0] +Torque_Limit = [1000] +Lock = [1] +Present_Position = [1904] +Present_Speed = [33018] +Present_Load = [0] +Present_Voltage = [123] +Present_Temperature = [44] +Status = [8] +Moving = [0] +Present_Current = [0] +Maximum_Acceleration = [1530] +-------------------------------------------------- +Timestamp: 1736435665.726 +Model = [2825] +ID = [1] +Baud_Rate = [0] +Return_Delay = [0] +Response_Status_Level = [1] +Min_Angle_Limit = [650] +Max_Angle_Limit = [2600] +Max_Temperature_Limit = [80] +Max_Voltage_Limit = [160] +Min_Voltage_Limit = [60] +Max_Torque_Limit = [1000] +Phase = [12] +Unloading_Condition = [45] +LED_Alarm_Condition = [45] +P_Coefficient = [32] +D_Coefficient = [32] +I_Coefficient = [0] +Minimum_Startup_Force = [15] +CW_Dead_Zone = [0] +CCW_Dead_Zone = [0] +Protection_Current = [310] +Angular_Resolution = [1] +Offset = [0] +Mode = [0] +Protective_Torque = [20] +Protection_Time = [200] +Overload_Torque = [80] +Speed_closed_loop_P_proportional_coefficient = [10] +Over_Current_Protection_Time = [50] +Velocity_closed_loop_I_integral_coefficient = [200] +Torque_Enable = [0] +Acceleration = [20] +Goal_Position = [600] +Goal_Time = [0] +Goal_Speed = [0] +Torque_Limit = [1000] +Lock = [1] +Present_Position = [1871] +Present_Speed = [33068] +Present_Load = [0] +Present_Voltage = [122] +Present_Temperature = [44] +Status = [8] +Moving = [0] +Present_Current = [0] +Maximum_Acceleration = [1530] +-------------------------------------------------- +Timestamp: 1736435665.857 +Model = [2825] +ID = [1] +Baud_Rate = [0] +Return_Delay = [0] +Response_Status_Level = [1] +Min_Angle_Limit = [650] +Max_Angle_Limit = [2600] +Max_Temperature_Limit = [80] +Max_Voltage_Limit = [160] +Min_Voltage_Limit = [60] +Max_Torque_Limit = [1000] +Phase = [12] +Unloading_Condition = [45] +LED_Alarm_Condition = [45] +P_Coefficient = [32] +D_Coefficient = [32] +I_Coefficient = [0] +Minimum_Startup_Force = [15] +CW_Dead_Zone = [0] +CCW_Dead_Zone = [0] +Protection_Current = [310] +Angular_Resolution = [1] +Offset = [0] +Mode = [0] +Protective_Torque = [20] +Protection_Time = [200] +Overload_Torque = [80] +Speed_closed_loop_P_proportional_coefficient = [10] +Over_Current_Protection_Time = [50] +Velocity_closed_loop_I_integral_coefficient = [200] +Torque_Enable = [0] +Acceleration = [20] +Goal_Position = [600] +Goal_Time = [0] +Goal_Speed = [0] +Torque_Limit = [1000] +Lock = [1] +Present_Position = [1839] +Present_Speed = [32968] +Present_Load = [0] +Present_Voltage = [123] +Present_Temperature = [44] +Status = [8] +Moving = [0] +Present_Current = [0] +Maximum_Acceleration = [1530] +-------------------------------------------------- +Timestamp: 1736435665.989 +Model = [2825] +ID = [1] +Baud_Rate = [0] +Return_Delay = [0] +Response_Status_Level = [1] +Min_Angle_Limit = [650] +Max_Angle_Limit = [2600] +Max_Temperature_Limit = [80] +Max_Voltage_Limit = [160] +Min_Voltage_Limit = [60] +Max_Torque_Limit = [1000] +Phase = [12] +Unloading_Condition = [45] +LED_Alarm_Condition = [45] +P_Coefficient = [32] +D_Coefficient = [32] +I_Coefficient = [0] +Minimum_Startup_Force = [15] +CW_Dead_Zone = [0] +CCW_Dead_Zone = [0] +Protection_Current = [310] +Angular_Resolution = [1] +Offset = [0] +Mode = [0] +Protective_Torque = [20] +Protection_Time = [200] +Overload_Torque = [80] +Speed_closed_loop_P_proportional_coefficient = [10] +Over_Current_Protection_Time = [50] +Velocity_closed_loop_I_integral_coefficient = [200] +Torque_Enable = [0] +Acceleration = [20] +Goal_Position = [600] +Goal_Time = [0] +Goal_Speed = [0] +Torque_Limit = [1000] +Lock = [1] +Present_Position = [1822] +Present_Speed = [32818] +Present_Load = [0] +Present_Voltage = [122] +Present_Temperature = [44] +Status = [8] +Moving = [0] +Present_Current = [0] +Maximum_Acceleration = [1530] +-------------------------------------------------- +Timestamp: 1736435666.121 +Model = [2825] +ID = [1] +Baud_Rate = [0] +Return_Delay = [0] +Response_Status_Level = [1] +Min_Angle_Limit = [650] +Max_Angle_Limit = [2600] +Max_Temperature_Limit = [80] +Max_Voltage_Limit = [160] +Min_Voltage_Limit = [60] +Max_Torque_Limit = [1000] +Phase = [12] +Unloading_Condition = [45] +LED_Alarm_Condition = [45] +P_Coefficient = [32] +D_Coefficient = [32] +I_Coefficient = [0] +Minimum_Startup_Force = [15] +CW_Dead_Zone = [0] +CCW_Dead_Zone = [0] +Protection_Current = [310] +Angular_Resolution = [1] +Offset = [0] +Mode = [0] +Protective_Torque = [20] +Protection_Time = [200] +Overload_Torque = [80] +Speed_closed_loop_P_proportional_coefficient = [10] +Over_Current_Protection_Time = [50] +Velocity_closed_loop_I_integral_coefficient = [200] +Torque_Enable = [0] +Acceleration = [20] +Goal_Position = [600] +Goal_Time = [0] +Goal_Speed = [0] +Torque_Limit = [1000] +Lock = [1] +Present_Position = [1821] +Present_Speed = [0] +Present_Load = [0] +Present_Voltage = [123] +Present_Temperature = [44] +Status = [8] +Moving = [0] +Present_Current = [0] +Maximum_Acceleration = [1530] +-------------------------------------------------- diff --git a/shoulder_pitch_log_200ms.png b/shoulder_pitch_log_200ms.png new file mode 100644 index 00000000..79181c20 Binary files /dev/null and b/shoulder_pitch_log_200ms.png differ diff --git a/shoulder_pitch_log_50ms.png b/shoulder_pitch_log_50ms.png new file mode 100644 index 00000000..a910a644 Binary files /dev/null and b/shoulder_pitch_log_50ms.png differ diff --git a/shoulder_pitch_parameters_plot.png b/shoulder_pitch_parameters_plot.png new file mode 100644 index 00000000..446db712 Binary files /dev/null and b/shoulder_pitch_parameters_plot.png differ