commit 3d3e8f792ee239fe143453ad58a3df3580e8f227 Author: STT Date: Fri Nov 26 15:48:54 2021 +0800 遥控板初版 diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100644 index 0000000..359bb53 --- /dev/null +++ b/.idea/.gitignore @@ -0,0 +1,3 @@ +# 默认忽略的文件 +/shelf/ +/workspace.xml diff --git a/.idea/.name b/.idea/.name new file mode 100644 index 0000000..f795279 --- /dev/null +++ b/.idea/.name @@ -0,0 +1 @@ +usv.py \ No newline at end of file diff --git a/.idea/USVControl.iml b/.idea/USVControl.iml new file mode 100644 index 0000000..74d515a --- /dev/null +++ b/.idea/USVControl.iml @@ -0,0 +1,10 @@ + + + + + + + + + + \ No newline at end of file diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000..105ce2d --- /dev/null +++ b/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml new file mode 100644 index 0000000..bb848f4 --- /dev/null +++ b/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml new file mode 100644 index 0000000..cd9e84c --- /dev/null +++ b/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/control.py b/control.py new file mode 100644 index 0000000..d90358b --- /dev/null +++ b/control.py @@ -0,0 +1,13 @@ +import struct +from remote_control_unit import RemoteControlUnit + + +class Control: + def __init__(self): + self.mode = ['lock', 'manual'] + self.data = {'mode': self.mode[1], 'thrust': 0, 'rudder': 0, } + + def run(self, usv): + if self.data['mode'] == self.mode[1]: + self.data['rudder'] = int(1.4881 * (usv.futaba.receive_data['channel1'] - 1024)) + self.data['thrust'] = int(-1.4881 * (usv.futaba.receive_data['channel3'] - 1024)) diff --git a/navigation.py b/navigation.py new file mode 100644 index 0000000..9864a6f --- /dev/null +++ b/navigation.py @@ -0,0 +1,5 @@ +class Navigation: + """组合导航模块""" + + def __init__(self): + pass diff --git a/remote_control_unit.py b/remote_control_unit.py new file mode 100644 index 0000000..3eaded6 --- /dev/null +++ b/remote_control_unit.py @@ -0,0 +1,98 @@ +import serial +import array + + +class RemoteControlUnit: + """遥控器相关属性及方法""" + + def __init__(self, com, ): + self.receive_data = {'channel1': 0, 'channel2': 0, 'channel3': 0, 'channel4': 0, 'channel5': 0, 'channel6': 0, + 'channel7': 0, 'channel8': 0, 'channel9': 0, 'channel10': 0, 'channel11': 0, + 'channel12': 0, 'channel13': 0, 'channel14': 0, 'channel15': 0, 'channel16': 0, 'flag': 0, + 'error': 0, } + self.send_data = self.receive_data.copy() + self.sbus = serial.Serial(com, 100000, serial.EIGHTBITS, serial.PARITY_EVEN, serial.STOPBITS_TWO, 0, False, + False, 0) + self.buffer = [] + self.packet = array.array('B') + + def run(self, usv): + self.buffer += self.sbus.read() + self.decode(usv) + + def decode(self, usv): + """解包数据""" + while len(self.buffer) >= 25: + if self.buffer[0] != 0x0f: + del self.buffer[0] + continue + + if self.buffer[24] != 0x04 and self.buffer[24] != 0x14 and self.buffer[24] != 0x24 and self.buffer[ + 24] != 0x34: + del self.buffer[0] + self.receive_data['error'] += 1 + continue + + self.receive_data['channel1'] = (self.buffer[1] >> 0 | self.buffer[2] << 8) & 0x07ff + self.receive_data['channel2'] = (self.buffer[2] >> 3 | self.buffer[3] << 5) & 0x07ff + self.receive_data['channel3'] = (self.buffer[3] >> 6 | self.buffer[4] << 2 | + self.buffer[5] << 10) & 0x07ff + self.receive_data['channel4'] = (self.buffer[5] >> 1 | self.buffer[6] << 7) & 0x07ff + self.receive_data['channel5'] = (self.buffer[6] >> 4 | self.buffer[7] << 4) & 0x07ff + self.receive_data['channel6'] = (self.buffer[7] >> 7 | self.buffer[8] << 1 | + self.buffer[9] << 9) & 0x07ff + self.receive_data['channel7'] = (self.buffer[9] >> 2 | self.buffer[10] << 6) & 0x07ff + self.receive_data['channel8'] = (self.buffer[10] >> 5 | self.buffer[11] << 3) & 0x07ff + self.receive_data['channel9'] = (self.buffer[12] >> 0 | self.buffer[13] << 8) & 0x07ff + self.receive_data['channel10'] = (self.buffer[13] >> 3 | self.buffer[14] << 5) & 0x07ff + self.receive_data['channel11'] = (self.buffer[14] >> 6 | self.buffer[15] << 2 | + self.buffer[16] << 10) & 0x07ff + self.receive_data['channel12'] = (self.buffer[16] >> 1 | self.buffer[17] << 7) & 0x07ff + self.receive_data['channel13'] = (self.buffer[17] >> 4 | self.buffer[18] << 4) & 0x07ff + self.receive_data['channel14'] = (self.buffer[18] >> 7 | self.buffer[19] << 1 | + self.buffer[20]) << 9 & 0x07ff + self.receive_data['channel15'] = (self.buffer[20] >> 2 | self.buffer[21] << 6) & 0x07ff + self.receive_data['channel16'] = (self.buffer[21] >> 5 | self.buffer[22] << 3) & 0x07ff + self.receive_data['flag'] = self.buffer[23] + + del self.buffer[:25] + + self.send_data = self.receive_data.copy() + self.send_data['channel1'] = int( + 1024 + ((usv.control.data['rudder'] + usv.control.data['thrust']) * 0.672)) # 左电机 + self.send_data['channel3'] = int( + 1024 + ((usv.control.data['thrust'] - usv.control.data['rudder']) * 0.672)) # 右电机 + + self.encode() + self.sbus.write(self.packet.tobytes()) + + break + + def encode(self): + """打包数据""" + self.packet = array.array('B') + self.packet.append(0x0f) + self.packet.append(self.send_data['channel1'] & 0xff) + self.packet.append((self.send_data['channel1'] >> 8 | self.send_data['channel2'] << 3) & 0xff) + self.packet.append((self.send_data['channel2'] >> 5 | self.send_data['channel3'] << 6) & 0xff) + self.packet.append((self.send_data['channel3'] >> 2) & 0xff) + self.packet.append((self.send_data['channel3'] >> 10 | self.send_data['channel4'] << 1) & 0xff) + self.packet.append((self.send_data['channel4'] >> 7 | self.send_data['channel5'] << 4) & 0xff) + self.packet.append((self.send_data['channel5'] >> 4 | self.send_data['channel6'] << 7) & 0xff) + self.packet.append((self.send_data['channel6'] >> 1) & 0xff) + self.packet.append((self.send_data['channel6'] >> 9 | self.send_data['channel7'] << 2) & 0xff) + self.packet.append((self.send_data['channel7'] >> 6 | self.send_data['channel8'] << 5) & 0xff) + self.packet.append((self.send_data['channel8'] >> 3) & 0xff) + self.packet.append(self.send_data['channel9'] & 0xff) + self.packet.append((self.send_data['channel9'] >> 8 | self.send_data['channel10'] << 3) & 0xff) + self.packet.append((self.send_data['channel10'] >> 5 | self.send_data['channel11'] << 6) & 0xff) + self.packet.append((self.send_data['channel11'] >> 2) & 0xff) + self.packet.append((self.send_data['channel11'] >> 10 | self.send_data['channel12'] << 1) & 0xff) + self.packet.append((self.send_data['channel12'] >> 7 | self.send_data['channel13'] << 4) & 0xff) + self.packet.append((self.send_data['channel13'] >> 4 | self.send_data['channel14'] << 7) & 0xff) + self.packet.append((self.send_data['channel14'] >> 1) & 0xff) + self.packet.append((self.send_data['channel14'] >> 9 | self.send_data['channel15'] << 2) & 0xff) + self.packet.append((self.send_data['channel15'] >> 6 | self.send_data['channel16'] << 5) & 0xff) + self.packet.append((self.send_data['channel16'] >> 3) & 0xff) + self.packet.append(self.send_data['flag']) + self.packet.append(0x04) diff --git a/settings.py b/settings.py new file mode 100644 index 0000000..eaaa8dc --- /dev/null +++ b/settings.py @@ -0,0 +1,6 @@ +class Settings: + """配置项""" + + def __init__(self): + # 串口配置 + self.sbus_com = 'COM50' diff --git a/usv.py b/usv.py new file mode 100644 index 0000000..344de24 --- /dev/null +++ b/usv.py @@ -0,0 +1,23 @@ +from remote_control_unit import RemoteControlUnit +from control import Control +from settings import Settings + + +class UsvControl: + """程序入口""" + + def __init__(self): + self.settings = Settings() + self.futaba = RemoteControlUnit(self.settings.sbus_com) + self.control = Control() + + def run(self): + while True: + self.futaba.run(self) + self.control.run(self) + + +# 按间距中的绿色按钮以运行脚本。 +if __name__ == '__main__': + usv1 = UsvControl() + usv1.run()