def __init__(self): super(window, self).__init__() self.ui = Ui_MainWindow() self.ui.setupUi(self) self.set_signals() self.update_target() zmq = Thread(target=self.run_zmq) zmq.start() self.cf = cf(self.ui) self.z = 0 self.x = 0 self.y = 0 self.theta = 0
def __init__(self): super(window, self).__init__() self.ui=Ui_MainWindow() self.ui.setupUi(self) self.set_signals() self.update_target() zmq=Thread(target=self.run_zmq) zmq.start() self.cf=cf(self.ui) self.z=0 self.x=0 self.y=0 self.theta=0
class window(QtGui.QMainWindow): def __init__(self): super(window, self).__init__() self.ui = Ui_MainWindow() self.ui.setupUi(self) self.set_signals() self.update_target() zmq = Thread(target=self.run_zmq) zmq.start() self.cf = cf(self.ui) self.z = 0 self.x = 0 self.y = 0 self.theta = 0 def set_signals(self): self.ui.pb_scan.clicked.connect(self.scan) self.ui.pb_conn.clicked.connect(self.pb_conn_clicked) self.ui.pb_disc.clicked.connect(self.pb_disc_clicked) self.ui.pb_start.clicked.connect(self.pb_start_clicked) self.ui.pb_stop.clicked.connect(self.pb_stop_clicked) self.ui.pb_update.clicked.connect(self.update_target) # ustawienie polaczenie def pb_conn_clicked(self): uri = self.ui.cb_device.currentText() self.ui.l_conn.setText("connecting to {}".format(uri)) self.cf.connect(uri) self.ui.pb_scan.setEnabled(False) self.ui.pb_conn.setEnabled(False) self.ui.cb_device.setEnabled(False) self.ui.pb_disc.setEnabled(True) self.ui.gb_target.setEnabled(True) self.ui.gb_control.setEnabled(True) # rozlacznie def pb_disc_clicked(self): # self.pb_stop_clicked() self.ui.l_conn.setText("Disconnecting ...") self.cf.close() self.ui.pb_scan.setEnabled(True) self.ui.cb_device.setEnabled(True) self.ui.pb_disc.setEnabled(False) self.ui.gb_target.setEnabled(False) self.ui.gb_control.setEnabled(False) # sterowanie def pb_start_clicked(self): self.ui.pb_start.setEnabled(False) self.ui.pb_stop.setEnabled(True) self.ui.pb_update.setEnabled(True) self.ui.pb_disc.setEnabled(False) self.update_target() self.ctrl_isTrue = True self.ctrl_thread = Thread(target=self.ctrl) self.ctrl_thread.start() def pb_stop_clicked(self): self.ui.pb_start.setEnabled(True) self.ui.pb_stop.setEnabled(False) self.ui.pb_update.setEnabled(False) # self.ui.pb_disc.setEnabled(True) self.ctrl_isTrue = False # sleep(0.1) self.ctrl_thread.join() def update_target(self): self.x_t = float(self.ui.en_x.text()) self.y_t = float(self.ui.en_y.text()) self.z_t = float(self.ui.en_z.text()) self.theta_t = float(self.ui.en_theta.text()) def ctrl(self): # timeout=0 # while self.ctrl_isTrue: # # if self.x==5 or self.y==5 or self.z==5 or self.theta==200: # thrust,roll,pitch,yaw = fuzzy([self.x_t,self.y_t,self.z_t,self.theta_t],[self.x_p,self.y_p,self.z_p,self.theta_p]) # timeout+=1 # print timeout # if timeout>=20: # self.ctrl_isTrue=False # break # else: # thrust,roll,pitch,yaw = fuzzy([self.x_t,self.y_t,self.z_t,self.theta_t],[self.x,self.y,self.z,self.theta]) # timeout=0 # thrust=thrust*600 # self.ui.l_thrust.setText("Thrust: {}".format(thrust)) # self.ui.l_roll.setText("roll: {}".format(roll)) # self.ui.l_pitch.setText("pitch: {}".format(pitch)) # self.ui.l_yaw.setText("yaw: {}".format(yaw)) # self.cf.update_ctrl(thrust,pitch,roll,yaw) sleep(0.05) self.cf.update_ctrl(20000, 0, 0, 0) self.ui.l_thrust.setText("Thrust: {}".format(0)) self.ui.l_roll.setText("roll: {}".format(0)) self.ui.l_pitch.setText("pitch: {}".format(0)) self.ui.l_yaw.setText("yaw: {}".format(0)) # funkcja wyszukiwania + wypelnienie cb def scan(self): # find USB devices dev = usb.core.find(find_all=True) try: a = cflib.crtp.scan_interfaces() self.ui.cb_device.clear() k = 0 kk = 0 for cfg in dev: if cfg.idVendor == 6421: for i in a: self.ui.cb_device.addItem(i[0].replace( str(kk), str(k), 1)) kk = k k += 1 if len(a) > 0: self.ui.pb_conn.setEnabled(True) else: self.ui.pb_conn.setEnabled(False) except Exception as e: self.ui.l_conn.setText(str(e)) def run_zmq(self): port = 2500 context = zmq.Context() print "Connecting to server..." socket = context.socket(zmq.SUB) socket.setsockopt_string(zmq.SUBSCRIBE, u"") socket.connect("tcp://192.168.2.103:%s" % port) while True: rcv = socket.recv_unicode() # rcv=str(rcv) if rcv != None and len(rcv) > 0: rcv = rcv.replace(",", ".") splited = rcv.split(";") if self.x != 5 or self.y != 5 or self.z != 5 or self.theta != 200: self.x_p = self.x self.y_p = self.y self.z_p = self.z self.theta_p = self.theta self.x = float(splited[0]) self.y = float(splited[1]) self.z = float(splited[2]) self.theta = float(splited[3]) # self.x_e=float(splited[0])-self.x_t # self.y_e=float(splited[1])-self.y_t # self.z_e=float(splited[2])-self.z_t # self.theta_e=float(splited[3])-self.theta_t self.ui.l_x.setText("x: {}".format(splited[0])) self.ui.l_y.setText("y: {}".format(splited[1])) self.ui.l_z.setText("z: {}".format(splited[2])) self.ui.l_theta.setText("kat: {}".format(splited[3]))
class window(QtGui.QMainWindow): def __init__(self): super(window, self).__init__() self.ui=Ui_MainWindow() self.ui.setupUi(self) self.set_signals() self.update_target() zmq=Thread(target=self.run_zmq) zmq.start() self.cf=cf(self.ui) self.z=0 self.x=0 self.y=0 self.theta=0 def set_signals(self): self.ui.pb_scan.clicked.connect(self.scan) self.ui.pb_conn.clicked.connect(self.pb_conn_clicked) self.ui.pb_disc.clicked.connect(self.pb_disc_clicked) self.ui.pb_start.clicked.connect(self.pb_start_clicked) self.ui.pb_stop.clicked.connect(self.pb_stop_clicked) self.ui.pb_update.clicked.connect(self.update_target) # ustawienie polaczenie def pb_conn_clicked(self): uri=self.ui.cb_device.currentText() self.ui.l_conn.setText("connecting to {}".format(uri)) self.cf.connect(uri) self.ui.pb_scan.setEnabled(False) self.ui.pb_conn.setEnabled(False) self.ui.cb_device.setEnabled(False) self.ui.pb_disc.setEnabled(True) self.ui.gb_target.setEnabled(True) self.ui.gb_control.setEnabled(True) # rozlacznie def pb_disc_clicked(self): # self.pb_stop_clicked() self.ui.l_conn.setText("Disconnecting ...") self.cf.close() self.ui.pb_scan.setEnabled(True) self.ui.cb_device.setEnabled(True) self.ui.pb_disc.setEnabled(False) self.ui.gb_target.setEnabled(False) self.ui.gb_control.setEnabled(False) # sterowanie def pb_start_clicked(self): self.ui.pb_start.setEnabled(False) self.ui.pb_stop.setEnabled(True) self.ui.pb_update.setEnabled(True) self.ui.pb_disc.setEnabled(False) self.update_target(); self.ctrl_isTrue=True self.ctrl_thread=Thread(target=self.ctrl) self.ctrl_thread.start() def pb_stop_clicked(self): self.ui.pb_start.setEnabled(True) self.ui.pb_stop.setEnabled(False) self.ui.pb_update.setEnabled(False) # self.ui.pb_disc.setEnabled(True) self.ctrl_isTrue=False # sleep(0.1) self.ctrl_thread.join() def update_target(self): self.x_t=float(self.ui.en_x.text()) self.y_t=float(self.ui.en_y.text()) self.z_t=float(self.ui.en_z.text()) self.theta_t=float(self.ui.en_theta.text()) def ctrl(self): # timeout=0 # while self.ctrl_isTrue: # # if self.x==5 or self.y==5 or self.z==5 or self.theta==200: # thrust,roll,pitch,yaw = fuzzy([self.x_t,self.y_t,self.z_t,self.theta_t],[self.x_p,self.y_p,self.z_p,self.theta_p]) # timeout+=1 # print timeout # if timeout>=20: # self.ctrl_isTrue=False # break # else: # thrust,roll,pitch,yaw = fuzzy([self.x_t,self.y_t,self.z_t,self.theta_t],[self.x,self.y,self.z,self.theta]) # timeout=0 # thrust=thrust*600 # self.ui.l_thrust.setText("Thrust: {}".format(thrust)) # self.ui.l_roll.setText("roll: {}".format(roll)) # self.ui.l_pitch.setText("pitch: {}".format(pitch)) # self.ui.l_yaw.setText("yaw: {}".format(yaw)) # self.cf.update_ctrl(thrust,pitch,roll,yaw) sleep(0.05) self.cf.update_ctrl(20000,0,0,0) self.ui.l_thrust.setText("Thrust: {}".format(0)) self.ui.l_roll.setText("roll: {}".format(0)) self.ui.l_pitch.setText("pitch: {}".format(0)) self.ui.l_yaw.setText("yaw: {}".format(0)) # funkcja wyszukiwania + wypelnienie cb def scan(self): # find USB devices dev = usb.core.find(find_all=True) try: a=cflib.crtp.scan_interfaces() self.ui.cb_device.clear() k=0 kk=0 for cfg in dev: if cfg.idVendor==6421: for i in a: self.ui.cb_device.addItem(i[0].replace(str(kk),str(k),1)) kk=k k+=1 if len(a)>0: self.ui.pb_conn.setEnabled(True) else: self.ui.pb_conn.setEnabled(False) except Exception as e: self.ui.l_conn.setText(str(e)) def run_zmq(self): port=2500 context = zmq.Context() print "Connecting to server..." socket = context.socket(zmq.SUB) socket.setsockopt_string(zmq.SUBSCRIBE, u"") socket.connect ("tcp://192.168.2.103:%s" % port) while True: rcv=socket.recv_unicode() # rcv=str(rcv) if rcv!=None and len(rcv)>0: rcv=rcv.replace(",",".") splited=rcv.split(";") if self.x!=5 or self.y!=5 or self.z!=5 or self.theta!=200: self.x_p=self.x self.y_p=self.y self.z_p=self.z self.theta_p=self.theta self.x=float(splited[0]) self.y=float(splited[1]) self.z=float(splited[2]) self.theta=float(splited[3]) # self.x_e=float(splited[0])-self.x_t # self.y_e=float(splited[1])-self.y_t # self.z_e=float(splited[2])-self.z_t # self.theta_e=float(splited[3])-self.theta_t self.ui.l_x.setText("x: {}".format(splited[0])) self.ui.l_y.setText("y: {}".format(splited[1])) self.ui.l_z.setText("z: {}".format(splited[2])) self.ui.l_theta.setText("kat: {}".format(splited[3]))