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)