Ejemplo n.º 1
0
    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])
                thrust, yaw, roll, pitch = control(self.z_t, self.z_p, self.Vz,
                                                   self.theta_p, self.x_d_t,
                                                   self.x_d_p, self.Vx_d,
                                                   self.y_d_t, self.y_d_p,
                                                   self.Vy_d)
                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])
                thrust, yaw, roll, pitch = control(self.z_t, self.z, self.Vz,
                                                   self.theta, self.x_d_t,
                                                   self.x_d, self.Vx_d,
                                                   self.y_d_t, self.y_d,
                                                   self.Vy_d)
                timeout = 0
            # roll=0
            # pitch=4
            yaw = 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)
            print self.Vz
        self.cf.update_ctrl(0, 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))
Ejemplo n.º 2
0
    def update_control(self):
        # watek liczacy sterowanie
        vzcam=0
        # zachowanie poprzednich wartosci sterowania jesli pojawi sie blad odczytu z kamer
        if (self.isCorrect==0.0) :
            self.thrust_control=self.thrust_control_previous
            self.roll_control=self.roll_control_previous
            self.pitch_control=self.pitch_control_previous
            self.yaw_control=self.yaw_control_previous
            self.timeout+=1
            print self.timeout
            if self.timeout>=15:
                self.control_package={
                    "thrust":0,
                    "pitch":0,
                    "roll":0,
                    "yaw":0
                }
                self.control_data_sig.emit(self.control_package)
                self.stop_clicked()
        else:

            # wyliczenie danych z filtru kalmna
            self.stan=self.kalman.licz(self.x_cam,self.y_cam,self.z_cam,self.roll_dron,self.pitch_dron,self.yaw_cam) #

            x=self.stan[0]
            y=self.stan[1]
            z=self.stan[2]
            roll=self.stan[3]
            pitch=self.stan[4]
            alfa=self.stan[5]
            vx=self.stan[6]
            vy=self.stan[7]
            vz=self.stan[8]
            vroll=self.stan[9]
            vpitch=self.stan[10]
            valfa=self.stan[11]

            self.x_dron=(self.x_target-x)*cos(alfa)+(self.y_target-y)*sin(alfa)
            self.y_dron=(self.y_target-y)*cos(alfa)-(self.x_target-x)*sin(alfa)
            self.z_dron=self.z_target-z
            self.vx_dron=vx*cos(alfa)+vy*sin(alfa)
            self.vy_dron=vy*cos(alfa)-vx*sin(alfa)
            self.vz_dron=vz
            self.alfa_dron=alfa
            self.valfa_dron=valfa

            # obliczenie sterowanie wykorzystujac sprzezenie od stanu
            T,Y,R,P=sprzezenie.control(self.z_dron,self.vz_dron,alfa,valfa,self.x_dron,self.vx_dron,self.y_dron,self.vy_dron,roll,pitch,vroll,vpitch)

            # obliczenie sterowanie wykorzystujac fuzzy logic
            Tf=fz((self.z_target-z)*100,self.vz_dron)
            Rf=-fd((self.y_dron),self.vy_dron)
            Pf=fd((self.x_dron),self.vx_dron)
            Yf=fyaw((self.yaw_target-alfa)*180/pi,valfa)

            # wybrane wartosci sterowania
            self.thrust_control=Tf
            self.pitch_control=Pf-P
            self.roll_control=Rf+R
            self.yaw_control=Yf
            self.timeout=0

            # wyslanie sterowania do drona
            self.control_package={
                "thrust":self.thrust_control*600,
                "pitch":self.pitch_control,
                "roll":self.roll_control,
                "yaw":self.yaw_control
            }
            if self.ui.cb_sendCtrl.isChecked():
                self.control_data_sig.emit(self.control_package)

        # logowanie danych
        str1=",".join([str(self.x_cam),str(self.y_cam),str(self.z_cam),str(self.yaw_cam),str(self.vzcam)])
        str2=",".join(str(i) for i in self.stan)
        str3=",".join([str(self.pitch_control),str(self.roll_control),str(self.yaw_control),str(self.thrust_control)])
        str4=",".join([str(self.x_dron),str(self.y_dron),str(self.z_dron)])
        str5=",".join([str(self.roll_dron),str(self.pitch_dron),str(self.yaw_dron)])
        str6=",".join([str(self.x_target),str(self.y_target),str(self.z_target),str(self.yaw_target)])
        self.strlist.append(",".join([str1,str2,str3,str4,str5,str6]))

        # przypisanie poprzedniego sterowania
        self.thrust_control_previous=self.thrust_control
        self.pitch_control_previous=self.pitch_control
        self.roll_control_previous=self.roll_control
        self.yaw_control_previous=self.yaw_control
        pass
Ejemplo n.º 3
0
    def update_control(self):
        # watek liczacy sterowanie
        vzcam = 0
        # zachowanie poprzednich wartosci sterowania jesli pojawi sie blad odczytu z kamer
        if (self.isCorrect == 0.0):
            self.thrust_control = self.thrust_control_previous
            self.roll_control = self.roll_control_previous
            self.pitch_control = self.pitch_control_previous
            self.yaw_control = self.yaw_control_previous
            self.timeout += 1
            print self.timeout
            if self.timeout >= 15:
                self.control_package = {
                    "thrust": 0,
                    "pitch": 0,
                    "roll": 0,
                    "yaw": 0
                }
                self.control_data_sig.emit(self.control_package)
                self.stop_clicked()
        else:

            # wyliczenie danych z filtru kalmna
            self.stan = self.kalman.licz(self.x_cam, self.y_cam, self.z_cam,
                                         self.roll_dron, self.pitch_dron,
                                         self.yaw_cam)  #

            x = self.stan[0]
            y = self.stan[1]
            z = self.stan[2]
            roll = self.stan[3]
            pitch = self.stan[4]
            alfa = self.stan[5]
            vx = self.stan[6]
            vy = self.stan[7]
            vz = self.stan[8]
            vroll = self.stan[9]
            vpitch = self.stan[10]
            valfa = self.stan[11]

            self.x_dron = (self.x_target - x) * cos(alfa) + (self.y_target -
                                                             y) * sin(alfa)
            self.y_dron = (self.y_target - y) * cos(alfa) - (self.x_target -
                                                             x) * sin(alfa)
            self.z_dron = self.z_target - z
            self.vx_dron = vx * cos(alfa) + vy * sin(alfa)
            self.vy_dron = vy * cos(alfa) - vx * sin(alfa)
            self.vz_dron = vz
            self.alfa_dron = alfa
            self.valfa_dron = valfa

            # obliczenie sterowanie wykorzystujac sprzezenie od stanu
            T, Y, R, P = sprzezenie.control(self.z_dron, self.vz_dron, alfa,
                                            valfa, self.x_dron, self.vx_dron,
                                            self.y_dron, self.vy_dron, roll,
                                            pitch, vroll, vpitch)

            # obliczenie sterowanie wykorzystujac fuzzy logic
            Tf = fz((self.z_target - z) * 100, self.vz_dron)
            Rf = -fd((self.y_dron), self.vy_dron)
            Pf = fd((self.x_dron), self.vx_dron)
            Yf = fyaw((self.yaw_target - alfa) * 180 / pi, valfa)

            # wybrane wartosci sterowania
            self.thrust_control = Tf
            self.pitch_control = Pf - P
            self.roll_control = Rf + R
            self.yaw_control = Yf
            self.timeout = 0

            # wyslanie sterowania do drona
            self.control_package = {
                "thrust": self.thrust_control * 600,
                "pitch": self.pitch_control,
                "roll": self.roll_control,
                "yaw": self.yaw_control
            }
            if self.ui.cb_sendCtrl.isChecked():
                self.control_data_sig.emit(self.control_package)

        # logowanie danych
        str1 = ",".join([
            str(self.x_cam),
            str(self.y_cam),
            str(self.z_cam),
            str(self.yaw_cam),
            str(self.vzcam)
        ])
        str2 = ",".join(str(i) for i in self.stan)
        str3 = ",".join([
            str(self.pitch_control),
            str(self.roll_control),
            str(self.yaw_control),
            str(self.thrust_control)
        ])
        str4 = ",".join([str(self.x_dron), str(self.y_dron), str(self.z_dron)])
        str5 = ",".join(
            [str(self.roll_dron),
             str(self.pitch_dron),
             str(self.yaw_dron)])
        str6 = ",".join([
            str(self.x_target),
            str(self.y_target),
            str(self.z_target),
            str(self.yaw_target)
        ])
        self.strlist.append(",".join([str1, str2, str3, str4, str5, str6]))

        # przypisanie poprzedniego sterowania
        self.thrust_control_previous = self.thrust_control
        self.pitch_control_previous = self.pitch_control
        self.roll_control_previous = self.roll_control
        self.yaw_control_previous = self.yaw_control
        pass