def run(self):
try:
self._serial.open()
except serial.SerialException:
_print('Cannot open port.', ERROR)
self._serial.close()
sys.exit()
def configure(self, port=None, speed=1000000):
_print("Configuring Serial Port", DEBUG)
self._serial.port = port
self._serial.baudrate = speed
self._serial.stopbits = serial.STOPBITS_ONE
self._serial.bytesize = serial.EIGHTBITS
self._serial.timeout = TIMEOUT_S
def print(self):
_print("Data: ", DEBUG)
_print("\tPreamble=" + hex(self.preamble), DEBUG)
_print("\tData Flags=" + hex(self.data_flags), DEBUG)
_print("\tPacket Number=" + str(self.packet_number), DEBUG)
_print_bytes("\tArgs=", self.args, DEBUG)
_print("\tMotion=" + str(self.motion), DEBUG)
def __init__(self,filename):
_print("Creating File: " + str(filename), DEBUG)
self.filename = filename
try:
self.f = open(filename, 'w')
except Exception as e:
_print("Could not open file.", ERROR)
sys.exit()
self.f.write(HEADER)
def main(port, ble, memory, filename):
"""Starts a data session."""
# Setup Serial
setup_serial(port)
# Setup File
setup_file(filename)
# Send Command
start_session(ble, memory)
# Get Response
response = get_response()
# Wait for data packets
while (True):
try:
# Get data
data = get_data()
data.save(g_file)
except KeyboardInterrupt:
_print("Keyboard Interrupt", DEBUG)
break
except error.MyError as e:
raise e
end_session()
# Wait for response packet
while (True):
try:
# Get Response
response = get_response()
break
except error.MyError:
continue
# Clean up
g_file.close()
def __init__(self):
notif = Notif()
notif.receive()
self.bytes = notif.get()
#check preabmle
self.preamble = self.bytes[0]
if (self.preamble != RESPONSE_PREAMBLE[0]):
_print("Invalid Response Preamble.", WARNING)
raise error.PreambleErr
#get opcode
self.opcode = self.bytes[1]
#get length and check
self.length = self.bytes[2]
#check arg length
self.args = self.bytes[3:3 + self.length]
self.print()
def receive(self):
#read up to next preamble, parse, and check
preamble_byte = g_serial.read_until(SERIAL_PREAMBLE)
if preamble_byte == b'':
_print("Serial Read Timeout.", DEBUG)
raise error.TimeoutErr
#check for misalignment
if preamble_byte != SERIAL_PREAMBLE and preamble_byte[
-1] != SERIAL_PREAMBLE[0]:
_print("Serial Packet Preamble Error: " + hex(preamble_byte),
ERROR)
raise error.PreambleErr
#read length byte, parse, and check
length_byte = g_serial.read_bytes(1)
length = int.from_bytes(length_byte, byteorder='little', signed=False)
#read notif bytes and trailer byte
self.notif_bytes = g_serial.read_bytes(length)
trailer_bytes = g_serial.read_bytes(len(SERIAL_TRAILER))
#build bytes for debugging
_print("Serial Packet Recieved.", DEBUG)
def __init__(self, payload):
_print("Sending Command, Payload=: " + str(payload), DEBUG)
self.bytes = COMMAND_PREAMBLE + payload
#check preabmle
self.preamble = self.bytes[0]
#get opcode
self.opcode = self.bytes[1]
#get length and check
self.length = self.bytes[2]
#check arg length
self.args = self.bytes[3:3 + self.length]
self.print()
notif = Notif()
notif.set(self.bytes)
notif.send()
def __init__(self):
notif = Notif()
notif.receive()
self.bytes = notif.get()
self.motion = None
#check preabmle
self.preamble = self.bytes[0]
if (self.preamble != DATA_PREAMBLE[0]):
_print("Invalid Data Preamble.", ERROR)
raise error.PreambleErr
#get opcode
self.data_flags = self.bytes[1]
#get length and check
self.packet_number = self.bytes[2]
#check arg length
self.args = self.bytes[3:]
self.parse()
self.print()
def process_image():
#global detector
if request.json:
data = request.json
_print("Request: %s" % data)
image_path = data.get("image_path")
pic = cv2.imread(image_path)
_print("Loading image : {}".format(image_path))
start_time = time.time()
result = detector.detect(image_path, keep_best=False)
_print("Detect Using : {:.2f} seconds".format(
(time.time() - start_time)))
_print(result)
if result is None:
_print("{} image cannot open!".format(image_path))
return json.dumps('{"response": "image cannot open!"}')
_print("%s objs found!" % len(result))
result_path = image_path.split('.')[0] + '_result.jpg'
detector.save(result_path)
have_yw = "yw" if len(result) > 0 else "myw"
response = {
"response": "%s %s" % (have_yw, result_path.replace("\\", "/"))
}
_print("Response: %s" % response)
_print("*-" * 40 + "\n")
return json.dumps(response)
else:
return json.dumps("no json received")
result = detector.detect(image_path, keep_best=False)
_print("Detect Using : {:.2f} seconds".format(
(time.time() - start_time)))
_print(result)
if result is None:
_print("{} image cannot open!".format(image_path))
return json.dumps('{"response": "image cannot open!"}')
_print("%s objs found!" % len(result))
result_path = image_path.split('.')[0] + '_result.jpg'
detector.save(result_path)
have_yw = "yw" if len(result) > 0 else "myw"
response = {
"response": "%s %s" % (have_yw, result_path.replace("\\", "/"))
}
_print("Response: %s" % response)
_print("*-" * 40 + "\n")
return json.dumps(response)
else:
return json.dumps("no json received")
if __name__ == "__main__":
_print("Start detection!")
detector = Detector(
config_file="E:/DL/det2/YJH/retinanet_R_50_FPN_1x.yaml",
class_names=['nest', 'floating'])
_print("*-" * 40 + "\n")
app.run(host='127.0.0.1', debug=False, port=7890)
def startSim(max_iter, state_d, pid_ks):
ii = 0
arr = [0]*15
if len(pid_ks) == 3:
arr[ii] = pid_ks[0]
arr[ii + 1] = pid_ks[1]
arr[ii + 2] = pid_ks[2]
pid_ks = arr
k = np.reshape(np.array(pid_ks), (5, 3))
saver = Saver()
collecter1 = []
collecter2 = []
collecter3 = []
collecter4 = []
state = {
"vx": 0,
"vy": 0,
"vz": 0,
"x": 0,
"y": 0,
"z": 0,
"wx": 0,
"wy": 0,
"wz": 0,
"gamma": 0, # крен
"psi": 0, # рыскание
"nu": 0 # тангаж
}
motors = [0, 0, 0, 0]
# print(k)
i = 0
# -- PIDS --
pid_dy = PID(k[0][0], k[0][1], k[0][2])
pid_dx = PID(k[1][0], k[1][1], k[1][2])
pid_dz = PID(k[2][0], k[2][1], k[2][2])
pid_gamma = PID(k[3][0], k[3][1], k[3][2])
pid_nu = PID(k[4][0], k[4][1], k[4][2])
pid_psi = PID(1, 1, 1)
pid_gamma.setLimits(-1, 1)
pid_nu.setLimits(-1, 1)
while i < max_iter:
state = world_physics(getNewState(state, motors))
saver.put(state)
P = sv.getFullP(motors)
pid_dx.setLimits((-1)*P, P)
Ux = (-1)*pid_dx.compute(state_d["x"] - state["x"])
pid_dz.setLimits((-1)*P, P)
Uz = pid_dz.compute(state_d["z"] - state["z"])
if P != 0:
state_d["gamma"] = np.clip(asin((Uz*cos(state_d["psi"]) + Ux*sin(state_d["psi"])) / P), -1, 1)
state_d["nu"] = np.clip(asin((Uz*sin(state_d["psi"]) + Ux*cos(state_d["psi"])) / P), -1, 1)
U1 = round(pid_dy.compute(state_d["y"] - state["y"]), 2)
U2 = round(pid_gamma.compute(state_d["gamma"] - state["gamma"]), 2)*U1*0.5
U3 = round(pid_nu.compute(state_d["nu"] - state["nu"]), 2)*U1*0.5
U4 = round(pid_psi.compute(state_d["psi"] - state["psi"]), 2)*U1*0.5
motors = [round(U1 - U2 + U4, 2), round(U1 + U3 - U4, 2), round(U1 + U2 + U4, 2), round(U1 - U3 - U4, 2)]
collecter1.append(U1)
collecter2.append(U2)
collecter3.append(U3)
collecter4.append(U4)
lgg._print("N____________________________", i)
lgg._print(motors)
lgg._print(state)
lgg._print(state_d)
lgg._print(U1, U2, U3, U4)
lgg._print(Ux, Uz)
# input()
i += 1
return [saver, collecter1, collecter2, collecter3, collecter4]
def start_session(ble, memory):
""" Sends a command to the peripheral """
_print("Staring Session.", DEBUG)
Command(payloads.START_SESSION_BLE)
def print(self):
_print("Response: ", APP)
_print("\tPreamble=" + hex(self.preamble), APP)
_print("\tOpcode=" + hex(self.opcode), APP)
_print("\tLength=" + str(self.length), APP)
_print_bytes("\tArgs=", self.args, APP)
def getNewState(state, W):
w1 = round(W[0] + W[2] - W[1] - W[3], 3)
w2 = round(W[1] + W[3] - W[0] - W[2], 3)
Mmx = Im*state["wz"]*w1
Mmz = Im*state["wx"]*w2
Mpx = Ip*state["wz"]*w1
Mpz = Ip*state["wx"]*w2
Mqx = (getP(W[2]) - getP(W[0]))*l
Mqz = (getP(W[1]) - getP(W[3]))*l
Mqy = getP(W[1]) + getP(W[3]) - getP(W[0]) - getP(W[2])
if abs(W[1] + W[3] - W[0] - W[2]) <= 0.1:
Mqy = 0
Mmx = 0
Mmz = 0
Mpx = 0
Mpz = 0
if abs(W[0] - W[2]) <= 0.1:
Mqx = 0
if abs(W[1] - W[3]) <= 0.1:
Mqz = 0
MRx = Mqx + Mmx + Mpx
MRy = Mqy
MRz = Mqz + Mmz + Mpz
P = getFullP(W)
A = {}
A["ax"] = ( P*((-1)*cos(state["gamma"])*cos(state["psi"])*sin(state["nu"]) + sin(state["gamma"])*sin(state["psi"])) - getDirection(state["vx"])*apha*state["vx"]**2 )/m
A["ay"] = (P*cos(state["gamma"])*cos(state["nu"]) - m*g - getDirection(state["vy"])*apha*state["vy"]**2)/m
A["az"] = ( P*(cos(state["gamma"])*sin(state["psi"])*sin(state["nu"]) + sin(state["gamma"])*cos(state["psi"])) - getDirection(state["vz"])*apha*state["vz"]**2)/m
A["wx_dot"] = Iyzx*state["wy"]*state["wz"] + MRx/Ix
A["wy_dot"] = Izxy*state["wx"]*state["wz"] + MRy/Iy
A["wz_dot"] = Ixyz*state["wx"]*state["wy"] + MRz/Iz
A["gamma_dot"] = state["wx"]*cos(state["nu"]) - state["wy"]*sin(state["nu"])
A["psi_dot"] = (state["wx"]*sin(state["nu"] - state["wy"]*cos(state["nu"]))) / cos(state["gamma"])
A["nu_dot"] = state["wz"] + sin(state["nu"])*tan(state["gamma"])*state["wx"] + cos(state["nu"])*tan(state["gamma"])*state["wy"]
lgg._print("A ============================>", A)
lgg._print(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>")
lgg._print(Iyzx*state["wy"]*state["wz"], MRx/Ix)
lgg._print(Mqy, Mqx, Mqz)
lgg._print(Mmx, Mmz, Mpx, Mpz)
lgg._print(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>")
return integrator(state, A)
def get_response():
""" Gets a response packet from peripheral """
_print("Getting Response.", DEBUG)
return Response()
def get_data():
""" Gets a data packet from peripheral """
_print("Getting Data.", DEBUG)
return Data()
def print(self):
_print(
"\rEuler: x={0:6.2f}, y={1:6.2f}, z={2:6.2f}".format(
self.roll, self.pitch, self.yaw), APP)
def end_session():
""" Ends the session by sending a command and closing file """
_print("Ending Session.", DEBUG)
Command(payloads.STOP_SESSION)
pass
def close(self):
_print("Closing File.", DEBUG)
self.f.close()
def send(self):
#send serial packet to central
g_serial.write(self.bytes)
_print("Serial Packet Sent.", DEBUG)
