def test_pid(mass=3, velocity=1, start_point=2, iterations=2000, time_step=0.01, wind_force=0.1, max_force=5): pid = Pid(1.5, 0.5, 0.5) current_position = start_point times = [] positions = [] winds = [] wind_f = [(random.random(), random.random()) for _ in range(0, 10)] def wind(time_step): result = 0 for x, y in wind_f: result += wind_force * (x - 0.5) * math.sin(y * 0.5 * time_step) return result for i in range(0, iterations): force = min( max(pid.update(current_position, delta_time=time_step), -max_force), max_force) force += wind(i * time_step) acceleration = force / mass velocity += acceleration current_position += velocity * time_step times.append(i * time_step) positions.append(current_position) winds.append(wind(i * time_step)) pyplot.plot(times, positions) pyplot.plot(times, [0] * len(times), linestyle="--") pyplot.plot(times, winds, linestyle=":") pyplot.show()
class AltitudeControllerNode(): """ Node for calculating the wrench needed in order to keep the altitude to the seabed constant. As only one DOF -namely z- is controlled, the other values have to be given by some other input, e.g. via joystick. This node subscribes to 'altitude', expecting a sensor_msgs/Range message and to 'wrench_input' which should be of type geometry_msgs/WrenchStamped. The node also receives std_msgs/Float32 messages on the topic 'altitude_request' to define the required altitude (setpoint). The node then calculates the force in z needed to control the AUV to have the desired altitude, replaces this value within wrench_input and publishes the result to 'wrench_output'. Internally a simple PID is used for control, its variables can be modified using dynamic_reconfigure. A service is provided to enable and disable the altitude control, by default it is disabled. Enabling sets the setpoint to the current altitude. """ def __init__(self, frequency): self.FEEDBACK_TIMEOUT = 2.0 self.wrench_input = WrenchStamped() self.setpoint_valid = False self.feedback_received = False self.enabled = False self.last_feedback = Range() self.enable_server = rospy.Service('~enable', auv_control_msgs.srv.EnableControl, self.enable) self.pid = Pid(0.0, 0.0, 0.0) # the right constants will self.k_f = 0.0 # be set through dynamic reconfigure self.server = dynamic_reconfigure.server.Server( AltitudeControllerConfig, self.reconfigure) self.pub = rospy.Publisher('~wrench_output', WrenchStamped) rospy.Subscriber('~wrench_input', WrenchStamped, self.wrenchInputCallback) rospy.Subscriber('~altitude_request', Float32, self.setpointCallback) period = rospy.rostime.Duration.from_sec(1.0 / frequency) self.timer = rospy.Timer(period, self.updateOutput) rospy.Subscriber('altitude', Range, self.altitudeCallback) rospy.loginfo( 'Listening for altitude feedback to be published on ' '%s...', rospy.resolve_name('altitude')) rospy.loginfo('Waiting for setpoint to be published on ' '%s...', rospy.resolve_name('~altitude_request')) def enable(self, request): """ Handles ROS service requests for enabling/disabling control. Returns current enabled status and setpoint. """ response = auv_control_msgs.srv.EnableControlResponse() if request.enable: if self.isFeedbackValid(): self.enabled = True self.pid.setSetpoint(self.last_feedback.range) self.setpoint_valid = True response.enabled = True else: rospy.logerr( "Cannot enable altitude control without valid feedback!") response.enabled = False else: self.enabled = False response.enabled = False response.current_setpoint = self.pid.getSetpoint() return response def reconfigure(self, config, level): """ Handles dynamic reconfigure requests. """ rospy.loginfo("Reconfigure request...") self.pid.k_p = config['Kp'] self.pid.k_i = config['Ki'] self.pid.k_d = config['Kd'] self.k_f = config['Kf'] rospy.loginfo("Reconfigured to (Kp, Ki, Kd, Kf) = (%f, %f, %f, %f)", self.pid.k_p, self.pid.k_i, self.pid.k_d, self.k_f) return config # Returns the updated configuration. def setpointCallback(self, setpoint): """ Change the setpoint of the controller. """ if not self.setpoint_valid: rospy.loginfo("First setpoint received.") self.setpoint_valid = True rospy.loginfo('Changed setpoint to: %s', setpoint.data) self.pid.setSetpoint(setpoint.data) def altitudeCallback(self, range_msg): self.last_feedback = range_msg def wrenchInputCallback(self, wrench): self.wrench_input = wrench def isFeedbackValid(self): feedback_in_time = (rospy.Time.now() - self.last_feedback.header.stamp ).to_sec() < self.FEEDBACK_TIMEOUT return feedback_in_time and (self.last_feedback.range > 0) def updateOutput(self, event): if self.setpoint_valid and self.enabled: wrench_output = copy.deepcopy(self.wrench_input) if self.isFeedbackValid(): dt = (event.current_real - event.last_real).to_sec() wrench_output.wrench.force.z = -self.pid.update( self.last_feedback.range, dt) + self.k_f if wrench_output.wrench.force.z > 1.0: wrench_output.wrench.force.z = 1.0 if wrench_output.wrench.force.z < -1.0: wrench_output.wrench.force.z = -1.0 else: rospy.logwarn( "Altitude feedback is invalid, setting force in z to zero." ) wrench_output.wrench.force.z = 0.0 wrench_output.header.stamp = rospy.Time.now() self.pub.publish(wrench_output)
class __AutonomyThread(threading.Thread): def __init__(self): super().__init__() self.running = True self.target_lat = 39 self.target_lon = 70 self.target_yaw = 0 self.target_alt = 0 self.pid_left = Pid(0.1, 0, 1.0, max_integral=1) self.pid_forward = Pid(0.1, 0, 1.0, max_integral=1) self.pid_yaw = Pid(0.01, 0.005, 0, max_integral=10) self.pid_up = Pid(0.001, 0.0005, 0.01, max_integral=1) self.prev_yaw_error = 0 self.aux_switch_was_flipped = False self.last_debug_print = time.time() self.last_gps_update = 0 self.enabled = False def calc_error(self): """ Calculates the distance the drone is from the current target latitude and longitude, in meters, in each direction. :return: (left_error, forward_error): left_error is how far the drone is, to the left, from the target. forward_error is how far the drone is, in the forward direction, from the target. Either error can be negative or positive. """ # TODO: Uncomment the real stuff and comment out the fake values error_magnitude = gps.findDistanceBetweenLatLon(self.target_lat, self.target_lon) error_angle = gps.findCurrentBearing(self.target_lat, self.target_lon) bearing = uavcontrol.get_compass_sensor() left_error = error_magnitude * math.sin(math.radians(bearing - error_angle)) forward_error = error_magnitude * math.cos(math.radians(bearing - error_angle)) return -left_error, -forward_error def reset_target(self, lat, lon, yaw, alt): self.target_lat = lat self.target_lon = lon self.target_yaw = yaw self.target_alt = alt self.pid_forward.reset() self.pid_left.reset() self.pid_yaw.reset() self.pid_up.reset() def run(self): time.sleep(1) self.reset_target(gps.latitude, gps.longitude, uavcontrol.get_compass_sensor(average=True, continuous=True), gps.altitude) while self.running: if not self.enabled: time.sleep(UAV_CONTROL_UPDATE_PERIOD) continue if uavcontrol.get_aux_input() and not self.aux_switch_was_flipped: self.aux_switch_was_flipped = True print("AUX SWITCH FLIPPED") self.reset_target(gps.latitude, gps.longitude, uavcontrol.get_compass_sensor(average=True, continuous=True)) elif not uavcontrol.get_aux_input(): self.aux_switch_was_flipped = False # Set throttle manually from remote control # throttle = uavcontrol.get_throttle_input() # uavcontrol.set_throttle(throttle) # print("set throttle to {}".format(throttle)) yaw_error = uavcontrol.get_compass_sensor(average=True, continuous=True) - self.target_yaw yaw_correction = self.pid_yaw.update(yaw_error) yaw_correction = constrain(yaw_correction, min_=-0.8, max_=0.8) # Positive yaw is right, so negate it to make it left uavcontrol.set_yaw_signal(-yaw_correction) # uavcontrol.set_yaw_signal(uavcontrol.get_yaw_input()) if time.time() - self.last_gps_update >= 1: self.last_gps_update = time.time() gps.updated = False left_error, forward_error = self.calc_error() up_error = gps.altitude - self.target_alt if math.sqrt(left_error ** 2 + forward_error ** 2) > config.ORIENTATION_ERROR_MARGIN: self.target_yaw = gps.findCurrentBearing(self.target_lat, self.target_lon) left_correction = self.pid_left.update(left_error) forward_correction = self.pid_forward.update(forward_error) up_correction = self.pid_up.update(up_error) left_correction = constrain(left_correction, min_=-0.8, max_=0.8) forward_correction = constrain(forward_correction, min_=-0.8, max_=0.8) up_correction += (config.DRONE_MASS * 9.81) / \ (config.MAX_THRUST * math.cos(math.radians(left_correction * config.MAX_ROLL)) * math.cos(math.radians(forward_correction * config.MAX_PITCH))) up_correction = constrain(up_correction, min_=0, max_=1.0) # Positive pitch is forward, so this is all good uavcontrol.set_pitch(forward_correction) # Positive roll is right, so negate it to make it left uavcontrol.set_roll(-left_correction) uavcontrol.set_throttle(up_correction) print("{:5.1f} Err: (L: {:7.3f}, F: {:7.3f}, Y: {:7.3f}, U: {:7.3f}), " "PID: (L: {:7.3f}, F: {:7.3f}, Y: {:7.3f}, U: {:7.3f})".format( time.time() % 100, left_error, forward_error, yaw_error, up_error, left_correction, forward_correction, yaw_correction, up_correction) ) # uavcontrol.set_pitch(uavcontrol.get_pitch_input()) # uavcontrol.set_roll(uavcontrol.get_roll_input()) time.sleep(UAV_CONTROL_UPDATE_PERIOD)
def test_pid_update(self): #baseline test output = PidOutputTest() p = Pid(output, 0.0) p.set_set_point(0.0) p.update(0) self.assertEqual(output.correction, 0) p.update(100) self.assertEqual(output.correction, 0) #test kp is working p = Pid(output, 1.0) p.update(0) self.assertEqual(output.correction, 0) p.update(100) self.assertEqual(output.correction, 1.0*-100) p.update(0) self.assertEqual(output.correction, 0) #test ki is working p = Pid(output, 0.0, kI = 1.0) p.update(0) self.assertEqual(output.correction, 0) p.update(100) p.update(50) self.assertEqual(output.correction, 1.0*-150) p.update(-150) self.assertEqual(output.correction, 0) #test izone p = Pid(output, 0.0, kI = 1.0, izone=100) p.update(0) self.assertEqual(output.correction, 0) p.update(100) p.update(50) self.assertEqual(output.correction, 1.0*-100) p.update(-100) self.assertEqual(output.correction, 0) p.update(0) self.assertEqual(output.correction, 0) p.update(-100) p.update(-50) self.assertEqual(output.correction, 1.0*100) p.update(100) self.assertEqual(output.correction, 0) #test kd is working p = Pid(output, 0.0, kD = 1.0) p.update(0) self.assertEqual(output.correction, 0) p.update(100) self.assertEqual(output.correction, -100) p.update(0) self.assertEqual(output.correction, 100) p.update(0) self.assertEqual(output.correction, 0) #test kf is working p = Pid(output, 0.0, kF = 1.0) p.set_set_point(0) p.update(0) self.assertEqual(output.correction, 0) p.set_set_point(100) p.update(0) self.assertEqual(output.correction, 100) p.set_set_point(0) p.update(0) self.assertEqual(output.correction, 0)
if int(OB.LAMBDA_SENSOR_ENABLED): SOND.activate_contactor() # ACTIVATE LAMBDASOND if int(OB.FAN_ENABLED): if not FAN.contactor: # ACTIVATE FAN logging.info('%s', "FAN Activate contactor") FAN.activate_contactor() TIME_LEFT_OF_BLOCK_TIMER = int((int(TEMP_SCREW_BLOCK_TIMER) + int(OB.BLOCK_TIME)) - time.time()) if time.time() > (int(TEMP_SCREW_BLOCK_TIMER) + int(OB.BLOCK_TIME)): # TIMER BELOW BLOCKTIMER? if FIRE.get_temp() <= float(OB.FIRE_SET_TEMP): # FIRE BELOW SET TEMP? if TANK.get_temp() <= float(OB.TANK_SET_TEMP): # TANK BELOW SET TEMP? if BOILER.get_temp() <= float(OB.BOILER_SET_TEMP): # BOILER BELOW SET TEMP? TEMP_SCREW_BLOCK_TIMER = time.time() SCREW.activate_for_time(int(OB.RUN_TIME_SCREW)) logging.info('%s%s%s', "Run screw for ", OB.RUN_TIME_SCREW, "sec") if FAN.contactor and SOND.contactor: pid = p.update(TANK.get_temp()) FAN.set_voltage(pid*(-0.1)+2) else: FAN.set_voltage(int(OB.FAN_SAFE_MODE_SPEED)) if time.time() > (int(TEMP_LOG_BLOCK_TIMER) + int(OB.LOG_BLOCK_TIME)): # LOG DEBUG DATA TEMP_LOG_BLOCK_TIMER = time.time() logging.debug('Temp: %s PID: %s FAN: %s RPM', TANK.get_temp(), str(pid*(-0.1)+2), int(FAN.get_rpm())) else: # MAN MODE if FAN.contactor: logging.info('%s', "FAN Deactivate contactor") FAN.deactivate_contactor() if S.input_get_value(OB.BUTTON_MAN_SCREW_FORWARD): # MAN SCREW SCREW.activate_contactor() logging.info('%s', "MAN Screw pressed") while S.input_get_value(OB.BUTTON_MAN_SCREW_FORWARD): pass