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