diff --git a/lerobot/common/motors/feetech/tables.py b/lerobot/common/motors/feetech/tables.py index ada8d08f..3e2d4ed9 100644 --- a/lerobot/common/motors/feetech/tables.py +++ b/lerobot/common/motors/feetech/tables.py @@ -2,9 +2,8 @@ FIRMWARE_MAJOR_VERSION = (0, 1) FIRMWARE_MINOR_VERSION = (1, 1) MODEL_NUMBER = (3, 2) -# See this link for STS3215 Memory Table: -# https://docs.google.com/spreadsheets/d/1GVs7W1VS1PqdhA1nW-abeyAHhTUxKUdR/edit?usp=sharing&ouid=116566590112741600240&rtpof=true&sd=true # data_name: (address, size_byte) +# http://doc.feetech.cn/#/prodinfodownload?srcType=FT-SMS-STS-emanual-229f4476422d4059abfb1cb0 STS_SMS_SERIES_CONTROL_TABLE = { # EPROM "Firmware_Major_Version": FIRMWARE_MAJOR_VERSION, # read-only @@ -36,7 +35,7 @@ STS_SMS_SERIES_CONTROL_TABLE = { "Protective_Torque": (34, 1), "Protection_Time": (35, 1), "Overload_Torque": (36, 1), - "Speed_closed_loop_P_proportional_coefficient": (37, 1), + "Velocity_closed_loop_P_proportional_coefficient": (37, 1), "Over_Current_Protection_Time": (38, 1), "Velocity_closed_loop_I_integral_coefficient": (39, 1), # SRAM @@ -44,21 +43,30 @@ STS_SMS_SERIES_CONTROL_TABLE = { "Acceleration": (41, 1), "Goal_Position": (42, 2), "Goal_Time": (44, 2), - "Goal_Speed": (46, 2), + "Goal_Velocity": (46, 2), "Torque_Limit": (48, 2), "Lock": (55, 1), "Present_Position": (56, 2), # read-only - "Present_Speed": (58, 2), # read-only + "Present_Velocity": (58, 2), # read-only "Present_Load": (60, 2), # read-only "Present_Voltage": (62, 1), # read-only "Present_Temperature": (63, 1), # read-only "Status": (65, 1), # read-only "Moving": (66, 1), # read-only "Present_Current": (69, 2), # read-only - # Not in the Memory Table - "Maximum_Acceleration": (85, 2), + "Goal_Position_2": (71, 2), # read-only + # Factory + "Moving_Velocity": (80, 1), + "Moving_Velocity_Threshold": (80, 1), + "DTs": (81, 1), # (ms) + "Velocity_Unit_factor": (82, 1), + "Hts": (83, 1), # (ns) valid for firmware >= 2.54, other versions keep 0 + "Maximum_Velocity_Limit": (84, 1), + "Maximum_Acceleration": (85, 1), + "Acceleration_Multiplier ": (86, 1), # Acceleration multiplier in effect when acceleration is 0 } +# http://doc.feetech.cn/#/prodinfodownload?srcType=FT-SCSCL-emanual-cbcc8ab2e3384282a01d4bf3 SCS_SERIES_CONTROL_TABLE = { # EPROM "Firmware_Major_Version": FIRMWARE_MAJOR_VERSION, # read-only @@ -66,7 +74,7 @@ SCS_SERIES_CONTROL_TABLE = { "Model_Number": MODEL_NUMBER, # read-only "ID": (5, 1), "Baud_Rate": (6, 1), - "Return_Delay": (7, 1), + "Return_Delay_Time": (7, 1), "Response_Status_Level": (8, 1), "Min_Position_Limit": (9, 2), "Max_Position_Limit": (11, 2), @@ -90,16 +98,23 @@ SCS_SERIES_CONTROL_TABLE = { "Acceleration": (41, 1), "Goal_Position": (42, 2), "Running_Time": (44, 2), - "Goal_Speed": (46, 2), + "Goal_Velocity": (46, 2), "Lock": (48, 1), "Present_Position": (56, 2), # read-only - "Present_Speed": (58, 2), # read-only + "Present_Velocity": (58, 2), # read-only "Present_Load": (60, 2), # read-only "Present_Voltage": (62, 1), # read-only "Present_Temperature": (63, 1), # read-only "Sync_Write_Flag": (64, 1), # read-only "Status": (65, 1), # read-only "Moving": (66, 1), # read-only + # Factory + "PWM_Maximum_Step": (78, 1), + "Moving_Velocity_Threshold*50": (79, 1), + "DTs": (80, 1), # (ms) + "Minimum_Velocity_Limit*50": (81, 1), + "Maximum_Velocity_Limit*50": (82, 1), + "Acceleration_2": (83, 1), # don't know what that is } STS_SMS_SERIES_BAUDRATE_TABLE = { @@ -157,7 +172,7 @@ MODEL_BAUDRATE_TABLE = { # Sign-Magnitude encoding bits STS_SMS_SERIES_ENCODINGS_TABLE = { "Homing_Offset": 11, - "Goal_Speed": 15, + "Goal_Velocity": 15, } MODEL_ENCODING_TABLE = { diff --git a/lerobot/common/robots/lekiwi/robot_lekiwi.py b/lerobot/common/robots/lekiwi/robot_lekiwi.py index 8f2f9037..2fbb664a 100644 --- a/lerobot/common/robots/lekiwi/robot_lekiwi.py +++ b/lerobot/common/robots/lekiwi/robot_lekiwi.py @@ -672,7 +672,7 @@ class LeKiwi: """ Reads the raw speeds for all wheels. Returns a dictionary with motor names: """ - raw_speeds = self.motor_bus.read("Present_Speed", self.motor_ids) + raw_speeds = self.motor_bus.read("Present_Velocity", self.motor_ids) return { "left_wheel": int(raw_speeds[0]), "back_wheel": int(raw_speeds[1]), @@ -684,9 +684,9 @@ class LeKiwi: Sends raw velocity commands (16-bit encoded values) directly to the motor bus. The order of speeds must correspond to self.motor_ids. """ - self.motor_bus.write("Goal_Speed", command_speeds, self.motor_ids) + self.motor_bus.write("Goal_Velocity", command_speeds, self.motor_ids) def stop(self): """Stops the robot by setting all motor speeds to zero.""" - self.motor_bus.write("Goal_Speed", [0, 0, 0], self.motor_ids) + self.motor_bus.write("Goal_Velocity", [0, 0, 0], self.motor_ids) print("Motors stopped.") diff --git a/lerobot/common/robots/mobile_manipulator.py b/lerobot/common/robots/mobile_manipulator.py index 33027791..20c807ff 100644 --- a/lerobot/common/robots/mobile_manipulator.py +++ b/lerobot/common/robots/mobile_manipulator.py @@ -684,7 +684,7 @@ class LeKiwi: """ Reads the raw speeds for all wheels. Returns a dictionary with motor names: """ - raw_speeds = self.motor_bus.read("Present_Speed", self.motor_ids) + raw_speeds = self.motor_bus.read("Present_Velocity", self.motor_ids) return { "left_wheel": int(raw_speeds[0]), "back_wheel": int(raw_speeds[1]), @@ -696,9 +696,9 @@ class LeKiwi: Sends raw velocity commands (16-bit encoded values) directly to the motor bus. The order of speeds must correspond to self.motor_ids. """ - self.motor_bus.write("Goal_Speed", command_speeds, self.motor_ids) + self.motor_bus.write("Goal_Velocity", command_speeds, self.motor_ids) def stop(self): """Stops the robot by setting all motor speeds to zero.""" - self.motor_bus.write("Goal_Speed", [0, 0, 0], self.motor_ids) + self.motor_bus.write("Goal_Velocity", [0, 0, 0], self.motor_ids) print("Motors stopped.")