feat: 超·延时自控算法

delay_meta.py

@@ -0,0 +1,20 @@
+'''
+@description: 摆烂的利用延时的自控程序
+'''
+
+import time
+import random
+from utils.auv_control import Ship
+
+def motion_plan():
+    auv = Ship()
+    auv.LED0ON()
+    time.sleep(1)
+    auv.LED0OFF()
+
+    auv.setTarget_Y_Speed(0.5)
+    time.sleep(10)
+    auv.setTarget_Y_Speed(0)
+
+if __name__ == "__main__":
+    motion_plan()

utils/auv_control.py

@@ -0,0 +1,116 @@
+import serial
+import time
+import math
+
+
+class Ship:
+    # @parameter badudrate 波特率
+    def __init__(self):
+        self.connect = serial.Serial("/dev/ttyAMA0",baudrate=9600,timeout=0.5)
+        
+        self.Target_Pitch_Angle = 0
+        self.Target_Roll_Angle = 0
+        self.Target_Yaw_Angle = 0
+        self.Target_Pitch_Palstance = 0
+        self.Target_Roll_Palstance = 0
+        self.Target_Yaw_Palstance = 0
+        self.Target_X = 0
+        self.Target_Y = 0
+        self.Target_Depth = 0
+        self.Target_Height = 0
+        self.Target_X_Speed = 0
+        self.Target_Y_Speed = 0
+        self.Target_Z_Speed = 0
+        self.Target_X_Acc = 0
+        self.Target_Y_Acc = 0
+        self.LED0 = False
+        self.LED1 = False
+      
+    def sendMessage(self,tmp:int , val:float):
+        end = 255
+        end = end.to_bytes(1,'big')
+        id = tmp.to_bytes(1, 'big')
+        try:
+           self.connect.write(id)
+           self.connect.write(str(float()).encode('ascii'))
+           self.connect.write(str(' ').encode('ascii'))
+           self.connect.write(end)
+        except:
+            return 
+    
+    def setSlopeAndInter(self , slope : float , val : float):
+        tmp = 0x02
+        end = 0xff
+        id = tmp.to_bytes(1, 'big')
+        mark = end.to_bytes(1, 'big')
+        self.connect.write(id)
+        self.connect.write(str(slope).encode('ascii'))
+        self.connect.write(str(' ').encode('ascii'))
+        self.connect.write(str(val).encode('ascii'))
+        self.connect.write(str(' ').encode('ascii'))    
+        self.connect.write(end)   
+ 
+    def LED0ON(self):
+        if self.LED0 == False:
+            self.LED0 = True
+            tmp = 0x10
+            self.sendMessage(tmp , 0.0)
+    
+    def LED0OFF(self):
+        if self.LED0 == True:
+            self.LED0 = False
+            tmp = 0x0f
+            self.sendMessage(tmp , 0.0)
+    
+    def LED1OFF(self):
+        if self.LED1 == True:
+            self.LED1 = False
+            tmp = 0x11
+            self.endMessage(tmp , 0.0)
+    
+    def LED1ON(self):
+        if self.LED1 == False:
+            self.LED1 = False
+            tmp = 0x12
+            self.sendMessage(tmp, 0.0)
+    
+    def setTarget_Depth(self , val : float):
+        tmp = 0x08
+        self.Target_Depth = val
+        self.sendMessage(tmp, val)
+    
+    def setTarget_Yaw_Angle(self, val : float):
+        tmp = 0x02
+        self.Target_Yaw_Angle = val
+        self.sendMessage(tmp, val)
+
+    def setTarget_X_Speed(self , val : float):
+        tmp = 0x0A
+        self.Target_X_Speed = val
+        self.sendMessage(tmp, val)
+    
+    def setTarget_Y_Speed(self , val : float):
+        tmp = 0x0B
+        self.Target_Y_Speed = val
+        self.sendMessage(tmp, val)
+
+
+    def sendAlpha(self , ch : int):
+        tmp = 0x13 + ch
+        self.sendMessage(tmp, 0.0)
+    
+    def setFinal(self):
+        tmp = 0x2d
+        self.sendMessage(tmp, 0.0)
+
+    def setForward_Thrust(self, val:float):
+        tmp = 0x2f
+        self.sendMessage(tmp, val)
+
+    def setSidesway_Thrust(self , val: float):
+        tmp = 0x30
+        self.sendMessage(tmp ,val)
+
+    def setYaw_Rotate_Torque(self , val : float):
+        tmp = 0x32
+        self.sendMessage(tmp, val)