def reset_callback(self, empty): """ Reset the desired and current poses of the drone and set desired velocities to zero """ self.current_position = Position(z=self.current_position.z) self.desired_position = Position(z=self.current_position.z) self.desired_velocity.x = 0 self.desired_velocity.y = 0
def position_control_callback(self, msg): """ Set whether or not position control is enabled """ self.position_control = msg.data if self.position_control: self.desired_position = Position(self.current_position.x,self.current_position.y,self.current_position.z) print "Position Control Activated Setpoint: ",self.desired_position if (self.position_control != self.last_position_control): print "Position Control", self.position_control self.last_position_control = self.position_control
def reset(self): ''' Set desired_position to be current position, set filtered_desired_velocity to be zero, and reset both the PositionPID and VelocityPID ''' # reset position control variables self.position_error = Error(0, 0, 0) self.desired_position = Position(self.current_position.x, self.current_position.y, 0.3) # reset velocity control_variables self.velocity_error = Error(0, 0, 0) self.desired_velocity = Velocity(0, 0, 0) # reset the pids self.pid.reset() self.lr_pid.reset() self.fb_pid.reset()
def reset(self): ''' Set desired_position to zero, set filtered_desired_velocity to be zero, and reset both the PositionPID and VelocityPID ''' # reset position control variables self.position_error = Error(0,0,0) self.desired_position = Position(0,0,0.3) # reset velocity control_variables self.velocity_error = Error(0,0,0) self.desired_velocity = Velocity(0,0,0) # reset the pids self.pid.reset() self.lr_pid.reset() self.fb_pid.reset() print "Self reset called, desired position: ",self.desired_position
def __init__(self): # Initialize the current and desired modes self.current_mode = Mode('DISARMED') self.desired_mode = Mode('DISARMED') # Initialize in velocity control self.position_control = False # Initialize the current and desired positions self.current_position = Position() self.desired_position = Position(z=0.3) self.last_desired_position = Position(z=0.3) # Initialize the position error self.position_error = Error() # Initialize the current and desired velocities self.current_velocity = Velocity() self.desired_velocity = Velocity() # Initialize the velocity error self.velocity_error = Error() # Set the distance that a velocity command will move the drone (m) self.desired_velocity_travel_distance = 0.1 # Set a static duration that a velocity command will be held self.desired_velocity_travel_time = 0.1 # Set a static duration that a yaw velocity command will be held self.desired_yaw_velocity_travel_time = 0.25 # Store the start time of the desired velocities self.desired_velocity_start_time = None self.desired_yaw_velocity_start_time = None # Initialize the primary PID self.pid = PID() # Initialize the error used for the PID which is vx, vy, z where vx and # vy are velocities, and z is the error in the altitude of the drone self.pid_error = Error() # Initialize the 'position error to velocity error' PIDs: # left/right (roll) pid self.lr_pid = PIDaxis(kp=10.0, ki=0.5, kd=5.0, midpoint=0, control_range=(-10.0, 10.0)) # front/back (pitch) pid self.fb_pid = PIDaxis(kp=10.0, ki=0.5, kd=5.0, midpoint=0, control_range=(-10.0, 10.0)) # Initialize the pose callback time self.last_pose_time = None # Initialize the desired yaw velocity self.desired_yaw_velocity = 0 # Initialize the current and previous roll, pitch, yaw values self.current_rpy = RPY() self.previous_rpy = RPY() # initialize the current and previous states self.current_state = State() self.previous_state = State() # Store the command publisher self.cmdpub = None # a variable used to determine if the drone is moving between disired # positions self.moving = False # a variable that determines the maximum magnitude of the position error # Any greater position error will overide the drone into velocity # control self.safety_threshold = 1.5 # determines if the position of the drone is known self.lost = False # determines if the desired poses are aboslute or relative to the drone self.absolute_desired_position = False # determines whether to use open loop velocity path planning which is # accomplished by calculate_travel_time self.path_planning = True