def ctrl(self):
timeout=0
thrust_p=0
roll_p=0
pitch_p=0
yaw_p=0
while self.ctrl_isTrue:
if (self.x==5 or self.y==5 or self.z==5) :
thrust=thrust_p
roll=roll_p
pitch=pitch_p
yaw=yaw_p
timeout+=1
print timeout
if timeout>=3:
self.cf.update_ctrl(0,0,0,0)
self.ctrl_isTrue=False
break
elif (self.x==0 and self.y==0 and self.z==0 and self.theta==0):
thrust=0
roll=0
pitch=0
yaw=0
else:
thrust=fz(self.z_t-self.z,self.Vz)
roll=fx(self.x_t-self.x,self.Vx)
pitch=fy(self.y_t-self.y,self.Vy)
yaw=ftheta(self.theta)
timeout=0
thrust_p=thrust
roll_p=roll
pitch_p=pitch
yaw_p=yaw
self.ui.l_thrust.setText("Thrust: {:.3f}".format(thrust))
self.ui.l_roll.setText("roll: {:.3f}".format(roll))
self.ui.l_pitch.setText("pitch: {:.3f}".format(pitch))
self.ui.l_yaw.setText("yaw: {:.3f}".format(yaw))
self.cf.update_ctrl(thrust*600,pitch,roll,yaw)
sleep(0.05)
self.cf.update_ctrl(0,0,0,0)
self.ui.l_thrust.setText("Thrust: {:f}".format(0))
self.ui.l_roll.setText("roll: {:f}".format(0))
self.ui.l_pitch.setText("pitch: {:f}".format(0))
self.ui.l_yaw.setText("yaw: {:f}".format(0))
def ctrl(self):
self.x_plot=np.array([0.0])
self.y_plot=np.array([0.0])
timeout=0
thrust_p=0
roll_p=0
pitch_p=0
yaw_p=0
while self.ctrl_isTrue:
if (self.x==5 or self.y==5 or self.z==5) :
thrust=thrust_p
roll=roll_p
pitch=pitch_p
yaw=yaw_p
timeout+=1
if timeout>=20:
self.ctrl_isTrue=False
break
elif (self.x==0 and self.y==0 and self.z==0 and self.theta==0):
thrust=0
roll=-4
pitch=0
yaw=0
else:
thrust=fz(self.z_t-self.z,self.Vz)
roll=0
pitch=0
yaw=0
timeout=0
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*600,pitch,roll,yaw)
self.save_plot_data(self.x_plot[-1]+0.05,self.rpy[2])
sleep(0.05)
self.cf.update_ctrl(0,0,0,0)
self.ui.l_thrust.setText("Thrust: {:f}".format(0))
self.ui.l_roll.setText("roll: {:f}".format(0))
self.ui.l_pitch.setText("pitch: {:f}".format(0))
self.ui.l_yaw.setText("yaw: {:f}".format(0))
def ctrl(self):
self.x_plot = np.array([0.0])
self.y_plot = np.array([0.0])
timeout = 0
thrust_p = 0
roll_p = 0
pitch_p = 0
yaw_p = 0
while self.ctrl_isTrue:
if (self.x == 5 or self.y == 5 or self.z == 5):
thrust = thrust_p
roll = roll_p
pitch = pitch_p
yaw = yaw_p
timeout += 1
if timeout >= 20:
self.ctrl_isTrue = False
break
elif (self.x == 0 and self.y == 0 and self.z == 0
and self.theta == 0):
thrust = 0
roll = -4
pitch = 0
yaw = 0
else:
thrust = fz(self.z_t - self.z, self.Vz)
roll = 0
pitch = 0
yaw = 0
timeout = 0
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 * 600, pitch, roll, yaw)
self.save_plot_data(self.x_plot[-1] + 0.05, self.rpy[2])
sleep(0.05)
self.cf.update_ctrl(0, 0, 0, 0)
self.ui.l_thrust.setText("Thrust: {:f}".format(0))
self.ui.l_roll.setText("roll: {:f}".format(0))
self.ui.l_pitch.setText("pitch: {:f}".format(0))
self.ui.l_yaw.setText("yaw: {:f}".format(0))
