diff --git a/delay_meta.py b/delay_meta.py new file mode 100644 index 0000000..1008bc2 --- /dev/null +++ b/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() diff --git a/utils/auv_control.py b/utils/auv_control.py new file mode 100644 index 0000000..adbd71a --- /dev/null +++ b/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)