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))
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
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