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
