117 lines
3.1 KiB
Python
117 lines
3.1 KiB
Python
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)
|