robocad-py 1.0.2.7__tar.gz → 1.1.1.0__tar.gz

{robocad-py-1.0.2.7 → robocad_py-1.1.1.0}/CHANGELOG.md

@@ -22,4 +22,7 @@
 - Thread joins fixed 2 :)
 
 ## 0.0.1.1
-- Library fully changed according to the new requirements
+- Library fully changed according to the new requirements
+
+## 1.0.3.0
+- Fully rewritten

robocad_py-1.1.1.0/MANIFEST.in

@@ -0,0 +1,2 @@
+recursive-include robocad *.py
+include CHANGELOG.md LICENSE

{robocad-py-1.0.2.7/robocad_py.egg-info → robocad_py-1.1.1.0}/PKG-INFO

@@ -1,18 +1,30 @@
-Metadata-Version: 2.1
+Metadata-Version: 2.4
 Name: robocad-py
-Version: 1.0.2.7
+Version: 1.1.1.0
 Summary: python lib for real and virtual robots
 Home-page: https://github.com/Soft-V/robocad-py
 Author: Airat Abdrakov
 Author-email: softvery@yandex.ru
 License: MIT
-Keywords: simulator,robotics,robot,3d,raspberry,control
+Keywords: simulator,robotics,robot,3d,raspberry,control,robocad
 Classifier: Development Status :: 5 - Production/Stable
 Classifier: Intended Audience :: Education
 Classifier: Operating System :: Microsoft :: Windows :: Windows 10
 Classifier: License :: OSI Approved :: MIT License
 Classifier: Programming Language :: Python :: 3
 License-File: LICENSE
+Requires-Dist: numpy
+Requires-Dist: funcad
+Dynamic: author
+Dynamic: author-email
+Dynamic: classifier
+Dynamic: description
+Dynamic: home-page
+Dynamic: keywords
+Dynamic: license
+Dynamic: license-file
+Dynamic: requires-dist
+Dynamic: summary
 
 Python library for real and virtual robots
 
@@ -41,3 +53,6 @@ Python library for real and virtual robots
 
 ## 0.0.1.1
 - Library fully changed according to the new requirements
+
+## 1.0.3.0
+- Fully rewritten

robocad_py-1.1.1.0/robocad/algaritm.py

@@ -0,0 +1,188 @@
+import signal
+
+from .internal.common.robot import Robot
+from .internal.common.robot_configuration import DefaultAlgaritmConfiguration
+from .internal.algaritm_internal import AlgaritmInternal
+
+
+class RobotAlgaritm(Robot):
+    def __init__(self, is_real_robot: bool = True, conf: DefaultAlgaritmConfiguration = None):
+        if conf is None: conf = DefaultAlgaritmConfiguration()
+
+        super().__init__(is_real_robot, conf)
+        self.__algaritm_internal = AlgaritmInternal(self, conf)
+        self.__reseted_yaw_val = 0.0
+
+        signal.signal(signal.SIGTERM, self.handler)
+        signal.signal(signal.SIGINT, self.handler)
+
+    def stop(self):
+        self.__algaritm_internal.stop()
+        self.write_log("Program stopped")
+
+    def handler(self, signum, _):
+        self.write_log("Program stopped from handler")
+        self.write_log('Signal handler called with signal' + str(signum))
+        self.stop()
+        raise SystemExit("Exited")
+
+    @property
+    def motor_speed_0(self):
+        return self.__algaritm_internal.speed_motor_0
+
+    @motor_speed_0.setter
+    def motor_speed_0(self, value):
+        self.__algaritm_internal.speed_motor_0 = value
+
+    @property
+    def motor_speed_1(self):
+        return self.__algaritm_internal.speed_motor_1
+
+    @motor_speed_1.setter
+    def motor_speed_1(self, value):
+        self.__algaritm_internal.speed_motor_1 = value
+
+    @property
+    def motor_speed_2(self):
+        return self.__algaritm_internal.speed_motor_2
+
+    @motor_speed_2.setter
+    def motor_speed_2(self, value):
+        self.__algaritm_internal.speed_motor_2 = value
+
+    @property
+    def motor_speed_3(self):
+        return self.__algaritm_internal.speed_motor_3
+
+    @motor_speed_3.setter
+    def motor_speed_3(self, value):
+        self.__algaritm_internal.speed_motor_3 = value
+
+    @property
+    def motor_enc_0(self):
+        return self.__algaritm_internal.enc_motor_0
+
+    @property
+    def motor_enc_1(self):
+        return self.__algaritm_internal.enc_motor_1
+
+    @property
+    def motor_enc_2(self):
+        return self.__algaritm_internal.enc_motor_2
+
+    @property
+    def motor_enc_3(self):
+        return self.__algaritm_internal.enc_motor_3
+    
+    @property
+    def yaw(self):
+        return self.__algaritm_internal.yaw
+    
+    @property
+    def pitch(self):
+        return self.__algaritm_internal.pitch
+    
+    @property
+    def roll(self):
+        return self.__algaritm_internal.roll
+    
+    @property
+    def us_1(self):
+        return self.__algaritm_internal.ultrasound_1
+
+    @property
+    def us_2(self):
+        return self.__algaritm_internal.ultrasound_2
+    
+    @property
+    def us_3(self):
+        return self.__algaritm_internal.ultrasound_3
+    
+    @property
+    def us_4(self):
+        return self.__algaritm_internal.ultrasound_4
+
+    @property
+    def analog_1(self):
+        return self.__algaritm_internal.analog_1
+
+    @property
+    def analog_2(self):
+        return self.__algaritm_internal.analog_2
+
+    @property
+    def analog_3(self):
+        return self.__algaritm_internal.analog_3
+
+    @property
+    def analog_4(self):
+        return self.__algaritm_internal.analog_4
+    
+    @property
+    def analog_5(self):
+        return self.__algaritm_internal.analog_5
+    
+    @property
+    def analog_6(self):
+        return self.__algaritm_internal.analog_6
+    
+    @property
+    def analog_7(self):
+        return self.__algaritm_internal.analog_7
+    
+    @property
+    def analog_8(self):
+        return self.__algaritm_internal.analog_8
+    
+    @property
+    def additional_servo_1(self):
+        return self.__algaritm_internal.additional_servo_1
+
+    @additional_servo_1.setter
+    def additional_servo_1(self, value):
+        self.__algaritm_internal.additional_servo_1 = value
+
+    @property
+    def additional_servo_2(self):
+        return self.__algaritm_internal.additional_servo_2
+
+    @additional_servo_2.setter
+    def additional_servo_2(self, value):
+        self.__algaritm_internal.additional_servo_2 = value
+
+    def set_pid_settings(self, use_pid, p, i, d):
+        self.__algaritm_internal.use_pid = use_pid
+        self.__algaritm_internal.p_pid = p
+        self.__algaritm_internal.i_pid = i
+        self.__algaritm_internal.d_pid = d
+
+    # num 1 or 2
+    def step_motor_move(self, num, steps: int, steps_per_second: int, direction: bool):
+        self.__algaritm_internal.step_motor_move(num, steps, steps_per_second, direction)
+
+    @property
+    def is_step_1_busy(self):
+        return self.__algaritm_internal.is_step_1_busy
+    
+    @property
+    def is_step_2_busy(self):
+        return self.__algaritm_internal.is_step_2_busy
+
+    @property
+    def titan_limits(self) -> list:
+        return [self.__algaritm_internal.limit_h_0, self.__algaritm_internal.limit_l_0,
+                self.__algaritm_internal.limit_h_1, self.__algaritm_internal.limit_l_1,
+                self.__algaritm_internal.limit_h_2, self.__algaritm_internal.limit_l_2,
+                self.__algaritm_internal.limit_h_3, self.__algaritm_internal.limit_l_3]
+
+    @property
+    def camera_image(self):
+        return self.__algaritm_internal.get_camera()
+    
+    @property
+    def lidar_data(self):
+        return self.__algaritm_internal.get_lidar()
+    
+    # port is from 1 to 8 included
+    def set_angle_servo(self, value: float, port: int):
+        self.__algaritm_internal.set_servo_angle(value, port - 1)

robocad_py-1.1.1.0/robocad/internal/algaritm_internal.py

@@ -0,0 +1,385 @@
+import struct
+import sys
+import os
+import time
+import numpy
+from threading import Thread
+
+from funcad.funcad import Funcad
+
+from .common.shared import LibHolder
+from .common.robot import Robot
+from .common.connection_base import ConnectionBase
+from .common.connection_sim import ConnectionSim
+from .common.connection_real import ConnectionReal
+from .common.updaters import RepkaUpdater
+from .common.robot_configuration import DefaultAlgaritmConfiguration
+
+
+class AlgaritmInternal:
+    def __init__(self, robot: Robot, conf: DefaultAlgaritmConfiguration):
+        self.__robot = robot
+
+        # from Titan
+        self.speed_motor_0: float = 0.0
+        self.speed_motor_1: float = 0.0
+        self.speed_motor_2: float = 0.0
+        self.speed_motor_3: float = 0.0
+        self.enc_motor_0: int = 0
+        self.enc_motor_1: int = 0
+        self.enc_motor_2: int = 0
+        self.enc_motor_3: int = 0
+        self.limit_l_0: bool = False
+        self.limit_h_0: bool = False
+        self.limit_l_1: bool = False
+        self.limit_h_1: bool = False
+        self.limit_l_2: bool = False
+        self.limit_h_2: bool = False
+        self.limit_l_3: bool = False
+        self.limit_h_3: bool = False
+        self.additional_servo_1: float = 0.0
+        self.additional_servo_2: float = 0.0
+        self.is_step_1_busy: bool = False
+        self.is_step_2_busy: bool = False
+        self.step_motor_1_steps: int = 0
+        self.step_motor_2_steps: int = 0
+        self.step_motor_1_steps_per_s: int = 0
+        self.step_motor_2_steps_per_s: int = 0
+        self.step_motor_1_direction: bool = False
+        self.step_motor_2_direction: bool = False
+        self.use_pid: bool = False
+        self.p_pid: float = 0.14
+        self.i_pid: float = 0.1
+        self.d_pid: float = 0.0
+
+        # from vmx
+        self.yaw: float = 0
+        self.yaw_unlim: float = 0
+        self.pitch: float = 0
+        self.pitch_unlim: float = 0
+        self.roll: float = 0
+        self.roll_unlim: float = 0
+        self.ultrasound_1: float = 0
+        self.ultrasound_2: float = 0
+        self.ultrasound_3: float = 0
+        self.ultrasound_4: float = 0
+        self.analog_1: int = 0
+        self.analog_2: int = 0
+        self.analog_3: int = 0
+        self.analog_4: int = 0
+        self.analog_5: int = 0
+        self.analog_6: int = 0
+        self.analog_7: int = 0
+        self.analog_8: int = 0
+
+        self.servo_angles: list = [0.0] * 8
+
+        self.__connection: ConnectionBase = None
+        if not self.__robot.on_real_robot:
+            pass
+            # self.__connection = ConnectionSim(self.__robot)
+            # self.__robocad_conn = RobocadConnection()
+            # self.__robocad_conn.start(self.__connection, self.__robot, self)
+        else:
+            updater = RepkaUpdater(self.__robot)
+            self.__connection = ConnectionReal(self.__robot, updater, conf)
+            self.__titan = TitanCOM()
+            self.__titan.start_com(self.__connection, self.__robot, self, conf)
+            self.__vmx = VMXSPI()
+            self.__vmx.start_spi(self.__connection, self.__robot, self, conf)
+
+    def stop(self):
+        self.__connection.stop()
+        if not self.__robot.on_real_robot:
+            pass
+            # if self.__robocad_conn is not None:
+                # self.__robocad_conn.stop()
+        else:
+            if self.__titan is not None:
+                self.__titan.stop()
+            if self.__vmx is not None:
+                self.__vmx.stop()
+
+    def get_camera(self):
+        return self.__connection.get_camera()
+    
+    def get_lidar(self):
+        return self.__connection.get_lidar()
+    
+    def set_servo_angle(self, angle: float, pin: int):
+        self.servo_angles[pin] = angle
+
+    def step_motor_move(self, num, steps: int, steps_per_second: int, direction: bool):
+        if num == 1:
+            self.step_motor_1_steps = steps
+            self.step_motor_1_steps_per_s = steps_per_second
+            self.step_motor_1_direction = direction
+        elif num == 2:
+            self.step_motor_2_steps = steps
+            self.step_motor_2_steps_per_s = steps_per_second
+            self.step_motor_2_direction = direction
+
+    
+class TitanCOM:
+    def __init__(self):
+        self.__th: Thread = None
+        self.__stop_th: bool = False
+
+    def start_com(self, connection: ConnectionReal, robot: Robot, robot_internal: AlgaritmInternal, conf: DefaultAlgaritmConfiguration) -> None:
+        self.__connection: ConnectionReal = connection
+        self.__robot: Robot = robot
+        self.__robot_internal: AlgaritmInternal = robot_internal
+        self.__conf: DefaultAlgaritmConfiguration = conf
+
+        self.__stop_th: bool = False
+        self.__th: Thread = Thread(target=self.com_loop)
+        self.__th.daemon = True
+        self.__th.start()
+
+    def stop(self):
+        self.__stop_th = True
+        if self.__th is not None:
+            self.__th.join()
+
+    def com_loop(self) -> None:
+        try:
+            com_result = self.__connection.com_ini(self.__conf.titan_port, self.__conf.titan_baud)
+            if com_result != 0:
+                self.__robot.write_log("Failed to open COM")
+                return
+
+            start_time: int = round(time.time() * 10000)
+            send_count_time: float = time.time()
+            comm_counter = 0
+            while not self.__stop_th:
+                tx_time: float = round(time.time() * 10000)
+                tx_data = self.set_up_tx_data()
+                self.__robot.robot_info.tx_com_time_dev = round(time.time() * 10000) - tx_time
+
+                rx_data: bytearray = self.__connection.com_rw(tx_data)
+
+                rx_time: float = round(time.time() * 10000)
+                self.set_up_rx_data(rx_data)
+                self.__robot.robot_info.rx_com_time_dev = round(time.time() * 10000) - rx_time
+
+                comm_counter += 1
+                if time.time() - send_count_time > 1:
+                    send_count_time = time.time()
+                    self.__robot.robot_info.com_count_dev = comm_counter
+                    comm_counter = 0
+
+                time.sleep(0.001)
+                self.__robot.robot_info.com_time_dev = round(time.time() * 10000) - start_time
+                start_time = round(time.time() * 10000)
+        except Exception as e:
+            self.__connection.com_stop()
+            exc_type, exc_obj, exc_tb = sys.exc_info()
+            file_name = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
+            self.__robot.write_log(" ".join(map(str, [exc_type, file_name, exc_tb.tb_lineno])))
+            self.__robot.write_log(str(e))
+
+    def set_up_rx_data(self, data: bytearray) -> None:
+        if data[0] == 1:
+            if data[40] == 222:
+                self.__robot_internal.enc_motor_0 = ((data[4] & 0xff) << 24) | ((data[3] & 0xff) << 16) | ((data[2] & 0xff) << 8) | (data[1] & 0xff)
+                self.__robot_internal.enc_motor_1 = ((data[8] & 0xff) << 24) | ((data[7] & 0xff) << 16) | ((data[6] & 0xff) << 8) | (data[5] & 0xff)
+                self.__robot_internal.enc_motor_2 = ((data[12] & 0xff) << 24) | ((data[11] & 0xff) << 16) | ((data[10] & 0xff) << 8) | (data[9] & 0xff)
+                self.__robot_internal.enc_motor_3 = ((data[16] & 0xff) << 24) | ((data[15] & 0xff) << 16) | ((data[14] & 0xff) << 8) | (data[13] & 0xff)
+
+                self.__robot_internal.limit_l_0 = Funcad.access_bit(data[17], 0)
+                self.__robot_internal.limit_h_0 = Funcad.access_bit(data[17], 1)
+                self.__robot_internal.limit_l_1 = Funcad.access_bit(data[17], 2)
+                self.__robot_internal.limit_h_1 = Funcad.access_bit(data[17], 3)
+                self.__robot_internal.limit_l_2 = Funcad.access_bit(data[17], 4)
+                self.__robot_internal.limit_h_2 = Funcad.access_bit(data[17], 5)
+                self.__robot_internal.limit_l_3 = Funcad.access_bit(data[17], 6)
+                self.__robot_internal.limit_h_3 = Funcad.access_bit(data[17], 7)
+
+                self.__robot_internal.is_step_1_busy = (data[18] != 0)
+                self.__robot_internal.is_step_2_busy = (data[19] != 0)
+        else:
+            self.__robot.write_log("received wrong data " + " ".join(map(str, data)))
+
+    def set_up_tx_data(self) -> bytearray:
+        tx_data: bytearray = bytearray([0] * 48)
+        tx_data[0] = 1
+
+        tx_data[1] = int(numpy.clip(self.__robot_internal.speed_motor_0, -100, 100)).to_bytes(1, 'big', signed = True)[0]
+        tx_data[2] = int(numpy.clip(self.__robot_internal.speed_motor_1, -100, 100)).to_bytes(1, 'big', signed = True)[0]
+        tx_data[3] = int(numpy.clip(self.__robot_internal.speed_motor_2, -100, 100)).to_bytes(1, 'big', signed = True)[0]
+        tx_data[4] = int(numpy.clip(self.__robot_internal.speed_motor_3, -100, 100)).to_bytes(1, 'big', signed = True)[0]
+
+        # for ProgramIsRunning and directions
+        tx_data[5] = int('11' +
+                        ("1" if self.__robot_internal.step_motor_1_direction else "0") +
+                        ("1" if self.__robot_internal.step_motor_2_direction else "0") + 
+                        ("1" if self.__robot_internal.use_pid else "0") + '001', 2)
+        
+        tx_data[6] = int(self.__robot_internal.additional_servo_1)
+        tx_data[7] = int(self.__robot_internal.additional_servo_2)
+
+        step1_steps: bytearray = Funcad.int_to_4_bytes(abs(self.__robot_internal.step_motor_1_steps))
+        tx_data[8] = step1_steps[0]
+        tx_data[9] = step1_steps[1]
+        tx_data[10] = step1_steps[2]
+        tx_data[11] = step1_steps[3]
+        step2_steps: bytearray = Funcad.int_to_4_bytes(abs(self.__robot_internal.step_motor_2_steps))
+        tx_data[12] = step2_steps[0]
+        tx_data[13] = step2_steps[1]
+        tx_data[14] = step2_steps[2]
+        tx_data[15] = step2_steps[3]
+
+        step1_steps_ps: bytearray = Funcad.int_to_4_bytes(abs(self.__robot_internal.step_motor_1_steps_per_s))
+        tx_data[16] = step1_steps_ps[0]
+        tx_data[17] = step1_steps_ps[1]
+        tx_data[18] = step1_steps_ps[2]
+        tx_data[19] = step1_steps_ps[3]
+        step2_steps_ps: bytearray = Funcad.int_to_4_bytes(abs(self.__robot_internal.step_motor_2_steps_per_s))
+        tx_data[20] = step2_steps_ps[0]
+        tx_data[21] = step2_steps_ps[1]
+        tx_data[22] = step2_steps_ps[2]
+        tx_data[23] = step2_steps_ps[3]
+
+        packed_p = struct.pack('<f', self.__robot_internal.p_pid)
+        tx_data[24] = packed_p[0]
+        tx_data[25] = packed_p[1]
+        tx_data[26] = packed_p[2]
+        tx_data[27] = packed_p[3]
+        packed_i = struct.pack('<f', self.__robot_internal.i_pid)
+        tx_data[28] = packed_i[0]
+        tx_data[29] = packed_i[1]
+        tx_data[30] = packed_i[2]
+        tx_data[31] = packed_i[3]
+        packed_d = struct.pack('<f', self.__robot_internal.d_pid)
+        tx_data[32] = packed_d[0]
+        tx_data[33] = packed_d[1]
+        tx_data[34] = packed_d[2]
+        tx_data[35] = packed_d[3]
+
+        tx_data[40] = 222
+
+        return tx_data
+    
+class VMXSPI:
+    def __init__(self):
+        self.__th: Thread = None
+        self.__stop_th: bool = False
+
+    def start_spi(self, connection: ConnectionReal, robot: Robot, robot_internal: AlgaritmInternal, conf: DefaultAlgaritmConfiguration) -> None:
+        self.__connection: ConnectionReal = connection
+        self.__robot: Robot = robot
+        self.__robot_internal: AlgaritmInternal = robot_internal
+        self.__conf: DefaultAlgaritmConfiguration = conf
+
+        self.__toggler: int = 0
+        self.__stop_th: bool = False
+        self.__th: Thread = Thread(target=self.spi_loop)
+        self.__th.daemon = True
+        self.__th.start()
+
+    def stop(self):
+        self.__stop_th = True
+        if self.__th is not None:
+            self.__th.join()
+
+    def spi_loop(self) -> None:
+        try:
+            spi_result = self.__connection.spi_ini(self.__conf.vmx_port, self.__conf.vmx_ch, self.__conf.vmx_speed, self.__conf.vmx_mode)
+            if spi_result != 0:
+                self.__robot.write_log("Failed to open SPI")
+                return
+
+            start_time: float = round(time.time() * 10000)
+            send_count_time: float = time.time()
+            comm_counter = 0
+            while not self.__stop_th:
+                tx_time: float = round(time.time() * 10000)
+                tx_list = self.set_up_tx_data()
+                self.__robot.robot_info.tx_spi_time_dev = round(time.time() * 10000) - tx_time
+
+                rx_list: bytearray = self.__connection.spi_rw(tx_list)
+
+                rx_time: float = round(time.time() * 10000)
+                self.set_up_rx_data(rx_list)
+                self.__robot.robot_info.rx_spi_time_dev = round(time.time() * 10000) - rx_time
+
+                comm_counter += 1
+                if time.time() - send_count_time > 1:
+                    send_count_time = time.time()
+                    self.__robot.robot_info.spi_count_dev = comm_counter
+                    comm_counter = 0
+
+                time.sleep(0.002)
+                self.__robot.robot_info.spi_time_dev = round(time.time() * 10000) - start_time
+                start_time = round(time.time() * 10000)
+        except (Exception, EOFError) as e:
+            self.__connection.spi_stop()
+            exc_type, exc_obj, exc_tb = sys.exc_info()
+            file_name = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
+            self.__robot.write_log(" ".join(map(str, [exc_type, file_name, exc_tb.tb_lineno])))
+            self.__robot.write_log(str(e))
+
+    def set_up_rx_data(self, data: bytearray) -> None:
+        if data[0] == 1:
+            self.__robot_internal.analog_1 = (data[2] & 0xff) << 8 | (data[1] & 0xff)
+            self.__robot_internal.analog_2 = (data[4] & 0xff) << 8 | (data[3] & 0xff)
+            self.__robot_internal.analog_3 = (data[6] & 0xff) << 8 | (data[5] & 0xff)
+            self.__robot_internal.analog_4 = (data[8] & 0xff) << 8 | (data[7] & 0xff)
+            self.__robot_internal.analog_5 = (data[10] & 0xff) << 8 | (data[9] & 0xff)
+            self.__robot_internal.analog_6 = (data[12] & 0xff) << 8 | (data[11] & 0xff)
+            self.__robot_internal.analog_7 = (data[14] & 0xff) << 8 | (data[13] & 0xff)
+        elif data[0] == 2:
+            self.__robot_internal.analog_8 = (data[2] & 0xff) << 8 | (data[1] & 0xff)
+
+            us1_ui: int = (data[4] & 0xff) << 8 | (data[3] & 0xff)
+            self.__robot_internal.ultrasound_1 = us1_ui / 100
+            us2_ui: int = (data[6] & 0xff) << 8 | (data[5] & 0xff)
+            self.__robot_internal.ultrasound_2 = us2_ui / 100
+            us3_ui: int = (data[8] & 0xff) << 8 | (data[7] & 0xff)
+            self.__robot_internal.ultrasound_3 = us3_ui / 100
+            us4_ui: int = (data[10] & 0xff) << 8 | (data[9] & 0xff)
+            self.__robot_internal.ultrasound_4 = us4_ui / 100
+        elif data[0] == 3:
+            yaw_ui: int = (data[2] & 0xff) << 8 | (data[1] & 0xff)
+            new_yaw = (yaw_ui / 100) * (1 if Funcad.access_bit(data[7], 1) else -1)
+            self.__robot_internal.yaw_unlim += self.calc_angle_unlim(new_yaw, self.__robot_internal.yaw)
+            self.__robot_internal.yaw = new_yaw
+
+            pitch_ui: int = (data[4] & 0xff) << 8 | (data[3] & 0xff)
+            new_pitch = (pitch_ui / 100) * (1 if Funcad.access_bit(data[7], 2) else -1)
+            self.__robot_internal.pitch_unlim += self.calc_angle_unlim(new_pitch, self.__robot_internal.pitch)
+            self.__robot_internal.pitch = new_pitch
+
+            roll_ui: int = (data[6] & 0xff) << 8 | (data[5] & 0xff)
+            new_roll = (roll_ui / 100) * (1 if Funcad.access_bit(data[7], 3) else -1)
+            self.__robot_internal.roll_unlim += self.calc_angle_unlim(new_roll, self.__robot_internal.roll)
+            self.__robot_internal.roll = new_roll
+
+            power: float = ((data[8] & 0xff) << 8 | (data[7] & 0xff)) / 100
+            self.__robot.power = power
+
+    def set_up_tx_data(self) -> bytearray:
+        tx_list: bytearray = bytearray([0x00] * 16)
+
+        if self.__toggler == 0:
+            tx_list[0] = 1
+
+            tx_list[1] = int(self.__robot_internal.servo_angles[0])
+            tx_list[2] = int(self.__robot_internal.servo_angles[1])
+            tx_list[3] = int(self.__robot_internal.servo_angles[2])
+            tx_list[4] = int(self.__robot_internal.servo_angles[3])
+            tx_list[5] = int(self.__robot_internal.servo_angles[4])
+            tx_list[6] = int(self.__robot_internal.servo_angles[5])
+            tx_list[7] = int(self.__robot_internal.servo_angles[6])
+            tx_list[8] = int(self.__robot_internal.servo_angles[7])
+        return tx_list
+
+    def calc_angle_unlim(self, new_angle: float, old_angle: float) -> float:
+        delta_angle = new_angle - old_angle
+        if delta_angle < -180:
+            delta_angle = 180 - old_angle
+            delta_angle += 180 + new_angle
+        elif delta_angle > 180:
+            delta_angle = (180 + old_angle) * -1
+            delta_angle += (180 - new_angle) * -1
+        return delta_angle