diff --git a/rcb4/armh7interface.py b/rcb4/armh7interface.py index 2fc148f..9ddd948 100644 --- a/rcb4/armh7interface.py +++ b/rcb4/armh7interface.py @@ -26,10 +26,6 @@ from rcb4.data import kondoh7_elf from rcb4.rcb4interface import CommandTypes from rcb4.rcb4interface import deg_to_servovector -from rcb4.rcb4interface import interpolate_currents -from rcb4.rcb4interface import interpolate_or_extrapolate_current_settings -from rcb4.rcb4interface import interpolate_or_extrapolate_temperature_settings -from rcb4.rcb4interface import interpolate_or_extrapolate_temperatures from rcb4.rcb4interface import rcb4_dof from rcb4.rcb4interface import RCB4Interface from rcb4.rcb4interface import ServoOnOffValues @@ -44,6 +40,8 @@ from rcb4.struct_header import ServoStruct from rcb4.struct_header import SystemStruct from rcb4.struct_header import WormmoduleStruct +from rcb4.temperature import get_setting_value_from_temperatures +from rcb4.temperature import setting_value_to_temperature from rcb4.units import convert_data from rcb4.usb_utils import reset_serial_port @@ -927,7 +925,7 @@ def send_stretch(self, value=127, servo_ids=None): def send_current_limit(self, current_limit_a=4.0, servo_ids=None): if servo_ids is None: servo_ids = self.servo_sorted_ids - current_setting = interpolate_or_extrapolate_current_settings(current_limit_a) + current_setting = int(current_limit_a * 10) if not isinstance(current_setting, (list, tuple)): current_setting = [current_setting] * len(servo_ids) byte_list = ( @@ -943,7 +941,7 @@ def send_current_limit(self, current_limit_a=4.0, servo_ids=None): def send_temperature_limit(self, temperature_limit_c=80, servo_ids=None): if servo_ids is None: servo_ids = self.servo_sorted_ids - temperature_setting = interpolate_or_extrapolate_temperature_settings(temperature_limit_c) + temperature_setting = get_setting_value_from_temperatures(temperature_limit_c) if not isinstance(temperature_setting, (list, tuple)): temperature_setting = [temperature_setting] * len(servo_ids) byte_list = ( @@ -1475,7 +1473,7 @@ def read_servo_current(self): servo_current_vector[inverted_servo_mask] -= 64 # Apply calculations only on non-zero values - estimated_current_vector = interpolate_currents(servo_current_vector) + estimated_current_vector = servo_current_vector / 10.0 estimated_current_vector[inverted_servo_mask] *= -1.0 return estimated_current_vector @@ -1486,7 +1484,7 @@ def read_current_limit(self, servo_ids=None): self.servo_param64(sid, ["current_limit"])["current_limit"] for sid in servo_ids ] - return interpolate_currents(current_vector) + return current_vector / 10.0 def switch_reading_servo_temperature(self, enable=True): if enable: @@ -1511,7 +1509,7 @@ def read_servo_temperature(self): # Apply calculations only on non-zero values estimated_temperatures = np.zeros_like(servo_temperatures, dtype=np.float32) if np.any(non_zero_mask): - estimated_temperatures[non_zero_mask] = interpolate_or_extrapolate_temperatures( + estimated_temperatures[non_zero_mask] = setting_value_to_temperature( servo_temperatures[non_zero_mask] ) return estimated_temperatures @@ -1523,7 +1521,7 @@ def read_temperature_limit(self, servo_ids=None): self.servo_param64(sid, ["temperature_limit"])["temperature_limit"] for sid in servo_ids ] - return interpolate_or_extrapolate_temperatures(temperature_vector) + return setting_value_to_temperature(temperature_vector) if __name__ == "__main__": diff --git a/rcb4/ics.py b/rcb4/ics.py index 29998c8..8754412 100644 --- a/rcb4/ics.py +++ b/rcb4/ics.py @@ -9,8 +9,7 @@ import serial.tools.list_ports import yaml -from rcb4.rcb4interface import interpolate_currents -from rcb4.rcb4interface import interpolate_or_extrapolate_temperatures +from rcb4.temperature import get_setting_value_from_temperatures degree_to_pulse = 29.633 @@ -92,10 +91,12 @@ def __init__(self, baudrate=1250000, yaml_path=None): self.servo_candidates = list(range(18)) self.servo_id_index = 0 self.servo_id = 0 + self.send_angle_pulse = None self.selected_index = 0 self.baudrate = baudrate self.timeout = 0.1 self.ics = None + self.is_continuous_rotation_mode = None self.servo_eeprom_params64 = [ ("fix-header", [1, 2]), ("stretch-gain", [3, 4]), @@ -147,6 +148,11 @@ def open_connection(self): current_baudrate = self.baud() if current_baudrate != self.baudrate: self.baud(self.baudrate) + try: + self.setup_rotation_mode() + self.set_speed(127) + except Exception as _: + pass return True except IndexError: continue @@ -154,14 +160,16 @@ def open_connection(self): def set_default_eeprom_param(self): self.set_param( - [5, 10, 11, 4, 7, 15, 0, 0, 0, 2, 1, 14, - 15, 10, 0, 6, 2, 12, 14, 12, 0, 13, 10, - 12, 0, 0, 0, 0, 0, 10, 1, 14, 1, 2, 9, 8, - 14, 13, 9, 13, 6, 14, 9, 11, 10, 12, 9, 5, - 0, 14, 0, 1, 0, 0, 13, 2, 0, 0, 3, 12, 7, 8, 15, 14],) - - def read_baud(self): - _, result = self.read_param() + [5, 10, 11, 4, 7, 15, 0, 0, 0, 2, # 0-9 + 1, 14, 15, 10, 0, 6, 2, 12, 14, 12, # 10-19 + 0, 13, 10, 12, 0, 0, 0, 0, 0, 10, # 20-29 + 1, 14, 1, 2, 9, 8, 14, 13, 9, 13, # 30-39 + 6, 14, 9, 11, 10, 12, 9, 5, 0, 14, # 40-49 + 0, 1, 0, 0, 13, 2, 0, 0, 3, 12, # 50-59 + 7, 8, 15, 14],) # 60-63 + + def read_baud(self, servo_id=None): + _, result = self.read_param(servo_id=servo_id) return result["baud"] def baud(self, baud=None, servo_id=None): @@ -175,7 +183,7 @@ def baud(self, baud=None, servo_id=None): if servo_id is None: servo_id = self.get_servo_id() - ics_param64, _ = self.read_param() + ics_param64, _ = self.read_param(servo_id=servo_id) if baud == 1250000: ics_param64[27] = 0 elif baud == 625000: @@ -187,7 +195,7 @@ def baud(self, baud=None, servo_id=None): self.set_param(ics_param64, servo_id) # Re-open the connection with the updated baud rate self.open_connection() - return self.read_baud() + return self.read_baud(servo_id=servo_id) def get_servo_id(self): self.ics.write(bytes([0xFF, 0x00, 0x00, 0x00])) @@ -207,7 +215,7 @@ def set_speed(self, speed, servo_id=None): if servo_id is None: servo_id = self.get_servo_id() self.ics.write(bytes([0xC0 | (servo_id & 0x1F), 0x02, speed])) - time.sleep(0.05) + time.sleep(0.01) v = self.ics.read(6) return v[5] @@ -219,6 +227,14 @@ def get_speed(self, servo_id=None): v = self.ics.read(5) return v[4] + def get_stretch(self, servo_id=None): + if servo_id is None: + servo_id = self.get_servo_id() + self.ics.write(bytes([0xA0 | (servo_id & 0x1F), 0x01])) + time.sleep(0.05) + v = self.ics.read(5) + return v[4] + def get_current(self, servo_id=None, interpolate=True): if servo_id is None: servo_id = self.get_servo_id() @@ -231,7 +247,7 @@ def get_current(self, servo_id=None, interpolate=True): current -= 64 sign = -1 if interpolate: - return sign * interpolate_currents(current) + return sign * current / 10.0 return sign * current def get_temperature(self, servo_id=None, interpolate=True): @@ -241,7 +257,7 @@ def get_temperature(self, servo_id=None, interpolate=True): time.sleep(0.05) v = self.ics.read(5) if interpolate: - return interpolate_or_extrapolate_temperatures(v[4]) + return get_setting_value_from_temperatures(v[4]) return v[4] def set_response(self, value, servo_id=None): @@ -270,6 +286,11 @@ def reset_servo_position(self): self.set_angle(7500) print(f"{Fore.YELLOW}Servo reset to zero position.{Fore.RESET}") + def setup_rotation_mode(self): + self.is_continuous_rotation_mode = self.read_rotation() + if self.is_continuous_rotation_mode: + self.send_angle_pulse = 7500 + def toggle_rotation_mode(self): rotation_mode = self.read_rotation() self.set_rotation(not rotation_mode) @@ -286,16 +307,48 @@ def set_free_mode(self): print(f"{Fore.MAGENTA}Free mode set to {mode_text}{Fore.RESET}") def increase_angle(self): - angle = self.read_angle() - angle = min(11500, angle + degree_to_pulse * 15) - self.set_angle(angle) - print(f"{Fore.BLUE}Angle increased to {angle}{Fore.RESET}") + if self.is_continuous_rotation_mode: + angle_pulse = self.send_angle_pulse + angle_pulse = min(11500, angle_pulse + int(degree_to_pulse * 1)) + else: + angle = self.read_angle() + angle_pulse = min(11500, angle + degree_to_pulse * 15) + self.set_angle(angle_pulse) + print(f"{Fore.BLUE}Angle increased to {angle_pulse}{Fore.RESET}") def decrease_angle(self): - angle = self.read_angle() - angle = max(3500, angle - degree_to_pulse * 15) - self.set_angle(angle) - print(f"{Fore.RED}Angle decreased to {angle}{Fore.RESET}") + if self.is_continuous_rotation_mode: + angle_pulse = self.send_angle_pulse + angle_pulse = max(3500, angle_pulse - int(degree_to_pulse * 1)) + else: + angle = self.read_angle() + angle_pulse = max(3500, angle - degree_to_pulse * 15) + self.set_angle(angle_pulse) + print(f"{Fore.RED}Angle decreased to {angle_pulse}{Fore.RESET}") + + def increase_speed(self): + speed = self.get_speed() + speed = min(speed + 10, 127) + self.set_speed(speed) + print(f"{Fore.BLUE}Speed increased to {speed}{Fore.RESET}") + + def decrease_speed(self): + speed = self.get_speed() + speed = max(1, speed - 10) + self.set_speed(speed) + print(f"{Fore.BLUE}Speed decreased to {speed}{Fore.RESET}") + + def increase_stretch(self): + stretch = self.get_stretch() + stretch = min(stretch + 10, 127) + self.set_stretch(stretch) + print(f"{Fore.BLUE}Stretch increased to {stretch}{Fore.RESET}") + + def decrease_stretch(self): + stretch = self.get_stretch() + stretch = max(1, stretch - 10) + self.set_stretch(stretch) + print(f"{Fore.BLUE}Stretch decreased to {stretch}{Fore.RESET}") def parse_param64_key_value(self, v): alist = {} @@ -303,13 +356,17 @@ def parse_param64_key_value(self, v): param_name, indices = param[0], param[1] alist[param_name] = self._4bit2num(indices, v) - baud_value = alist.get("ics-baud-rate-10-115200-00-1250000", 0) + baud_value = alist.get("ics-baud-rate-10-115200-00-1250000", 0) & 0x0F + servo_type = alist.get("ics-baud-rate-10-115200-00-1250000", 0) & 0xF0 baud_rate = {10: 115200, 1: 625000, 0: 1250000}.get(baud_value, None) mode_flag_value = alist.get( "mode-flag-b7slave-b4rotation-b3pwm-b1free-b0reverse", 0 ) alist.update(self.ics_flag_dict(mode_flag_value)) - alist.update({"servo-id": alist.get("servo-id", 0), "baud": baud_rate}) + alist.update({"servo-id": alist.get("servo-id", 0), "baud": baud_rate, + "ics-baud-rate-10-115200-00-1250000": baud_value, + "custom-servo-type": servo_type}) + # custom-servo-type is original parameter. return alist def _4bit2num(self, lst, v): @@ -352,9 +409,21 @@ def set_slave(self, slave=None, servo_id=None): return self.set_flag("slave", slave, servo_id=servo_id) def set_rotation(self, rotation=None, servo_id=None): + if rotation: + self.send_angle_pulse = 7500 + self.set_free(True, servo_id=servo_id) # free for before mode change. return self.set_flag("rotation", rotation, servo_id=servo_id) - def set_stretch(self, stretch_values, servo_id=None): + def set_stretch(self, value, servo_id=None): + if servo_id is None: + servo_id = self.get_servo_id() + value = max(1, min(value, 127)) + self.ics.write(bytes([0xC0 | servo_id, 0x01, value])) + time.sleep(0.1) + v = self.ics.read(6) + return v[2] + + def set_stretch_values(self, stretch_values, servo_id=None): if servo_id is None: servo_id = self.get_servo_id() @@ -415,6 +484,7 @@ def read_free(self, servo_id=None): def read_rotation(self, servo_id=None): _, result = self.read_param(servo_id=servo_id) + self.is_continuous_rotation_mode = result["rotation"] return result["rotation"] def set_param(self, ics_param64, servo_id=None): @@ -435,6 +505,8 @@ def display_status(self): options = [ "Current Servo ID", "Angle", + "Speed", + "Stretch", "Baud Rate", "Rotation Mode", "Slave Mode", @@ -442,7 +514,7 @@ def display_status(self): "Serial Mode", "Free", ] - selectable_options = ["Current Servo ID", "Angle"] + selectable_options = ["Current Servo ID", "Angle", "Speed", "Stretch"] key_listener = KeyListener() key_listener.daemon = True @@ -491,12 +563,16 @@ def display_status(self): sys.stdout.flush() print("--- Servo Status ---") for i, option in enumerate(options): - if i == self.selected_index: - print( - f">> {option}: {self.get_status(option, result, selected=True)}" - ) + selected = i == self.selected_index + if selected: + prefix_str = ">> " else: - print(f" {option}: {self.get_status(option, result, selected=False)}") + prefix_str = " " + try: + print_str = self.get_status(option, result, selected=selected) + except Exception as _: + print_str = 'No Data' + print(f"{prefix_str} {option}: {print_str}") print("----------------------\n") print( @@ -585,6 +661,26 @@ def display_status(self): and selectable_options[self.selected_index] == "Angle" ): self.increase_angle() + elif ( + key == readchar.key.LEFT + and selectable_options[self.selected_index] == "Speed" + ): + self.decrease_speed() + elif ( + key == readchar.key.RIGHT + and selectable_options[self.selected_index] == "Speed" + ): + self.increase_speed() + elif ( + key == readchar.key.LEFT + and selectable_options[self.selected_index] == "Stretch" + ): + self.decrease_stretch() + elif ( + key == readchar.key.RIGHT + and selectable_options[self.selected_index] == "Stretch" + ): + self.increase_stretch() else: use_previous_result = True except Exception as e: @@ -622,9 +718,24 @@ def get_status(self, option, param=None, selected=False): s += f' -> Next Servo ID: {str}' return s elif option == "Angle": - angle = self.read_angle() + if self.is_continuous_rotation_mode: + angle = self.send_angle_pulse + else: + angle = self.read_angle() angle = int((angle - 7500) / degree_to_pulse) return f"{angle}" + elif option == "Stretch": + if param is not None: + stretch = param["stretch-gain"] // 2 + else: + stretch = self.get_stretch() + return f"{stretch}" + elif option == "Speed": + if param is not None: + speed = param["speed"] + else: + speed = self.get_speed() + return f"{speed}" elif option == "Baud Rate": if param is not None: baudrate = param["baud"] @@ -675,6 +786,7 @@ def set_angle(self, v=7500, servo_id=None): v = int(v) if servo_id is None: servo_id = self.get_servo_id() + self.send_angle_pulse = v self.ics.write(bytes([0x80 | (servo_id & 0x1F), (v >> 7) & 0x7F, v & 0x7F])) time.sleep(0.1) v = self.ics.read(6) diff --git a/rcb4/rcb4interface.py b/rcb4/rcb4interface.py index 4939b6a..edbfc12 100644 --- a/rcb4/rcb4interface.py +++ b/rcb4/rcb4interface.py @@ -13,166 +13,6 @@ from rcb4.asm import rcb4_checksum from rcb4.asm import rcb4_velocity -# Known data points for temperature and setting values -temperature_data = np.array([100, 90, 80, 70, 60], dtype=np.float32) -temperature_setting_values_data = np.array([30, 47, 60, 75, 87], dtype=np.float32) -# Known data points for current and setting values -current_data = np.array([0.0, 0.1, 0.5, 1.0, 1.5, 2.0], dtype=np.float32) # in Amps -current_setting_values_data = np.array([0, 1, 5, 10, 15, 20], dtype=np.float32) # setting values - - -def interpolate_or_extrapolate_temperatures(setting_values): - """Approximation of temperatures for an array of setting values.""" - setting_values = np.atleast_1d(setting_values) # Ensure setting_values is an array - - # Calculate slopes for interpolation between points - slopes = (temperature_data[1:] - temperature_data[:-1]) / ( - temperature_setting_values_data[1:] - temperature_setting_values_data[:-1] - ) - - # Find indices where each setting value would fit within temperature_setting_values_data - indices = np.searchsorted(temperature_setting_values_data, setting_values, side="right") - 1 - - # Handle in-range values (interpolation) - in_range = (indices >= 0) & (indices < len(slopes)) - interpolated_values = temperature_data[indices[in_range]] + slopes[ - indices[in_range] - ] * (setting_values[in_range] - temperature_setting_values_data[indices[in_range]]) - - # Initialize result array with zeros (or any placeholder values) - temperatures = np.zeros_like(setting_values, dtype=float) - - # Set interpolated values - temperatures[in_range] = interpolated_values - - # Extrapolate for values below the minimum setting value - below_range = setting_values < temperature_setting_values_data[0] - temperatures[below_range] = temperature_data[0] + slopes[0] * ( - setting_values[below_range] - temperature_setting_values_data[0] - ) - - # Extrapolate for values above the maximum setting value - above_range = setting_values > temperature_setting_values_data[-1] - temperatures[above_range] = temperature_data[-1] + slopes[-1] * ( - setting_values[above_range] - temperature_setting_values_data[-1] - ) - - # If input was a single value, return a single float instead of an array - return temperatures if len(temperatures) > 1 else temperatures[0] - - -def interpolate_currents(setting_values): - """Approximation of current values for an array of setting values.""" - setting_values = np.atleast_1d(setting_values) # Ensure setting_values is an array - - # Calculate slopes for interpolation between points - slopes = (current_data[1:] - current_data[:-1]) / ( - current_setting_values_data[1:] - current_setting_values_data[:-1] - ) - - # Find indices where each setting value would fit within current_setting_values_data - indices = np.searchsorted(current_setting_values_data, setting_values, side="right") - 1 - - # Initialize result array with zeros (or any placeholder values) - current_values = np.zeros_like(setting_values, dtype=float) - - # Interpolate within range - in_range = (indices >= 0) & (indices < len(slopes)) - current_values[in_range] = current_data[indices[in_range]] + slopes[ - indices[in_range] - ] * (setting_values[in_range] - current_setting_values_data[indices[in_range]]) - - # Extrapolate for values below the minimum setting value - below_range = setting_values < current_setting_values_data[0] - current_values[below_range] = current_data[0] + slopes[0] * ( - setting_values[below_range] - current_setting_values_data[0] - ) - - # Extrapolate for values above the maximum setting value - above_range = setting_values > current_setting_values_data[-1] - current_values[above_range] = current_data[-1] + slopes[-1] * ( - setting_values[above_range] - current_setting_values_data[-1] - ) - - # If input was a single value, return a single float instead of an array - return current_values if len(current_values) > 1 else current_values[0] - - -def interpolate_or_extrapolate_temperature_settings(temperatures): - """Approximation of setting values for an array of temperatures.""" - temperatures = np.atleast_1d(temperatures) # Ensure temperatures is an array - - # Calculate slopes for interpolation between points - slopes = (temperature_setting_values_data[1:] - temperature_setting_values_data[:-1]) / ( - temperature_data[1:] - temperature_data[:-1] - ) - - # Find indices where each temperature would fit within temperature_data - indices = np.searchsorted(temperature_data[::-1], temperatures, side="right") - 1 - indices = len(temperature_data) - 1 - indices # Reverse indices for original order - - # Initialize result array with zeros (or any placeholder values) - setting_values = np.zeros_like(temperatures, dtype=float) - - # Interpolate within range - in_range = (indices >= 0) & (indices < len(slopes)) - setting_values[in_range] = temperature_setting_values_data[indices[in_range]] + slopes[ - indices[in_range] - ] * (temperatures[in_range] - temperature_data[indices[in_range]]) - - # Extrapolate for values below the minimum temperature - below_range = temperatures > temperature_data[0] - setting_values[below_range] = temperature_setting_values_data[0] + slopes[0] * ( - temperatures[below_range] - temperature_data[0] - ) - - # Extrapolate for values above the maximum temperature - above_range = temperatures < temperature_data[-1] - setting_values[above_range] = temperature_setting_values_data[-1] + slopes[-1] * ( - temperatures[above_range] - temperature_data[-1] - ) - - # If input was a single value, return a single float instead of an array - return setting_values if len(setting_values) > 1 else setting_values[0] - - -def interpolate_or_extrapolate_current_settings(currents): - """Approximation of setting values for an array of currents.""" - currents = np.atleast_1d(currents) # Ensure currents is an array - - # Calculate slopes for interpolation between points - slopes = (current_setting_values_data[1:] - current_setting_values_data[:-1]) / ( - current_data[1:] - current_data[:-1] - ) - - # Find indices where each current would fit within current_data - indices = np.searchsorted(current_data, currents, side="right") - 1 - - # Initialize result array with zeros (or any placeholder values) - setting_values = np.zeros_like(currents, dtype=float) - - # Interpolate within range - in_range = (indices >= 0) & (indices < len(slopes)) - setting_values[in_range] = current_setting_values_data[indices[in_range]] + slopes[ - indices[in_range] - ] * (currents[in_range] - current_data[indices[in_range]]) - - # Extrapolate for values below the minimum current - below_range = currents < current_data[0] - setting_values[below_range] = current_setting_values_data[0] + slopes[0] * ( - currents[below_range] - current_data[0] - ) - - # Extrapolate for values above the maximum current - above_range = currents > current_data[-1] - setting_values[above_range] = current_setting_values_data[-1] + slopes[-1] * ( - currents[above_range] - current_data[-1] - ) - - # If input was a single value, return a single float instead of an array - return setting_values if len(setting_values) > 1 else setting_values[0] - - # 4000.0 / 135 deg_to_servovector = 29.62962962962963 diff --git a/rcb4/temperature.py b/rcb4/temperature.py new file mode 100644 index 0000000..a214585 --- /dev/null +++ b/rcb4/temperature.py @@ -0,0 +1,138 @@ +import numpy as np + +setting_value_to_temperature = np.array([ + 118, + 117.05882263183594, + 116.47058868408203, + 115.88235473632812, + 115.29412078857422, + 114.70588684082031, + 114.11764526367188, + 113.52941131591797, + 112.94117736816406, + 112.35294342041016, + 111.76470947265625, + 111.17646789550781, + 110.5882339477539, + 110.0, + 109.4117660522461, + 108.82353210449219, + 108.23529052734375, + 107.64705657958984, + 107.05882263183594, + 106.47058868408203, + 105.88235473632812, + 105.29412078857422, + 104.70587921142578, + 104.11764526367188, + 103.52941131591797, + 102.94117736816406, + 102.35294342041016, + 101.76470947265625, + 101.17646789550781, + 100.5882339477539, + 100.0, + 99.4117660522461, + 98.82353210449219, + 98.23529052734375, + 97.64705657958984, + 97.05882263183594, + 96.47058868408203, + 95.88235473632812, + 95.29412078857422, + 94.70587921142578, + 94.11764526367188, + 93.52941131591797, + 92.94117736816406, + 92.35294342041016, + 91.76470947265625, + 91.17646789550781, + 90.5882339477539, + 90.0, + 89.23076629638672, + 88.46154022216797, + 87.69230651855469, + 86.92308044433594, + 86.15384674072266, + 85.38461303710938, + 84.61538696289062, + 83.84615325927734, + 83.07691955566406, + 82.30769348144531, + 81.53845977783203, + 80.76923370361328, + 80.0, + 79.33333587646484, + 78.66666412353516, + 78.0, + 77.33333587646484, + 76.66666412353516, + 76.0, + 75.33333587646484, + 74.66666412353516, + 74.0, + 73.33333587646484, + 72.66666412353516, + 72.0, + 71.33333587646484, + 70.66666412353516, + 70.0, + 69.16666412353516, + 68.33333587646484, + 67.5, + 66.66666412353516, + 65.83333587646484, + 65.0, + 64.16666412353516, + 63.33333206176758, + 62.5, + 61.66666793823242, + 60.833335876464844, + 60.0, + 59.16666793823242, + 58.333335876464844, + 57.5, + 56.66666793823242, + 55.833335876464844, + 55.0, + 54.16666793823242, + 53.333335876464844, + 52.5, + 51.66666793823242, + 50.833335876464844, + 50.0, + 49.16666793823242, + 48.333335876464844, + 47.5, + 46.66666793823242, + 45.833335876464844, + 45.0, + 44.16666793823242, + 43.333335876464844, + 42.5, + 41.66666793823242, + 40.833335876464844, + 40.0, + 39.16666793823242, + 38.333335876464844, + 37.5, + 36.66666793823242, + 35.833335876464844, + 35.0, + 34.16666793823242, + 33.333335876464844, + 32.5, + 31.666667938232422, + 30.833335876464844, + 30.0, + 29.166667938232422, + 28.333335876464844, + 27.5, + 26.666667938232422, +]) + + +def get_setting_value_from_temperatures(temperatures): + indices = np.searchsorted(-setting_value_to_temperature, -temperatures, side='left') + valid_indices = (0 <= indices) & (indices < len(setting_value_to_temperature)) + return np.clip(np.where(valid_indices, indices + 1, 127), 0, 127)