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 __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 __init__(self):
self.state = defState
self.pos = position(0, 0, 0, False, False, False)
self.output = {
"roll": midOutput,
"pitch": midOutput,
"yaw": midOutput,
"throttle": minOutput
}
self.speeds = {
"roll": defSpeed,
"pitch": defSpeed,
"yaw": defSpeed,
"throttle": defSpeed
}
self.xPid = Pid(pidDefKp, pidDefKi, pidDefKd, 0, pidMinOut, pidMaxOut)
self.yPid = Pid(pidDefKp, pidDefKi, pidDefKd, 0, pidMinOut, pidMaxOut)
self.zPid = Pid(pidDefKp, pidDefKi, pidZKd, 0, pidMinOut, pidMaxOut)
self.setPoints = {"x": 0, "y": 0, "z": 0}
def start(self, Rover, target_deg, spin):
Tip.target_deg = target_deg
Tip.total_deg = abs(Rover.yaw - target_deg)
Tip.spin = spin
Tip.started = True
Tip.did_start_brake = False
Tip.start_yaw = Rover.yaw
Tip.Pid = Pid(Tip.kp, Tip.ki, Tip.kd, target_deg)
Rover.throttle = 0
print("Set rover steer to %d" % Rover.steer)
def start(self, Rover, target_pt):
Goto.target_pt = target_pt
Goto.start_pt = Rover.pos
Goto.total_dist = euclidean_heuristic(Goto.start_pt, Goto.target_pt)
Goto.last_dist = 0
Goto.started = True
Goto.quit_counter = 0
Goto.do_finishing_brake = False
Goto.Pid = Pid(Goto.kp, Goto.ki, Goto.kd, 0.0)
Rover.steer = 0
def __init__(self, frequency):
self.FEEDBACK_TIMEOUT = 1.0
self.setpoint_valid = False
self.feedback_received = False
self.enabled = False
self.last_feedback = Odometry()
self.enable_server = rospy.Service('~enable', auv_control_msgs.srv.EnableControl, self.enable)
self.pids = []
for i in range(6):
self.pids.append(Pid(0.0, 0.0, 0.0))
self.server = dynamic_reconfigure.server.Server(TwistControllerConfig, self.reconfigure)
self.pub = rospy.Publisher('~wrench_output', WrenchStamped)
rospy.Subscriber('~twist_request', TwistStamped, self.setpointCallback)
period = rospy.rostime.Duration.from_sec(1.0/frequency)
self.timer = rospy.Timer(period, self.updateOutput)
rospy.Subscriber('odometry', Odometry, self.odometryCallback)
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('~twist_request'))
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()
def main():
rospy.init_node('pid_controller', anonymous=True)
long_control = Pid()
# lat_control.angle_pred()
rospy.spin()
import cv2
import sys
from gui import render_crosshairs
from pid import Pid, print_graph
from red_blob_detection import RedBlobDetector
# Set to false if you don't want to show any windows
showGUI = True
controller = Pid(kp=0.2, ki=0.05, kd=0.2)
vision_algorithm = RedBlobDetector()
def move_camera(vehicle, pwm):
if vehicle:
pwm = max(pwm,
1) # Ensure we never ask for negative or zero pwm values
msg = vehicle.message_factory.rc_channels_override_encode(
1, 1, 0, 0, 0, 0, 0, pwm, 0, 0)
vehicle.send_mavlink(msg)
vehicle.flush()
def disable_camera(vehicle):
if vehicle:
msg = vehicle.message_factory.rc_channels_override_encode(
1, 1, 0, 0, 0, 0, 0, 0, 0, 0)
vehicle.send_mavlink(msg)
vehicle.flush()
rospy.Subscriber('cmd_pose', Pose, callback_pos)
rospy.Subscriber("odom", Odometry, callback_odom)
rospy.Subscriber('manual', Bool, callback_manual)
rate = rospy.Rate(30)
val = Cart_values()
vel_setp = Twist()
vel_curr = Twist()
pos_setp = Pose()
pos_curr = Pose()
kp_vel = rospy.get_param('kp_vel')
ki_vel = rospy.get_param('ki_vel')
kd_vel = rospy.get_param('kd_vel')
kp_turn = rospy.get_param('kp_turn')
ki_turn = rospy.get_param('ki_turn')
kd_turn = rospy.get_param('kd_turn')
pid_vel = Pid(kp_vel, ki_vel, kd_vel)
pid_turn = Pid(kp_turn, ki_turn, kd_turn)
unwrapper = Unwrapper(math.pi)
def hardware_controller():
while not rospy.is_shutdown():
rate.sleep()
if __name__ == '__main__':
try:
hardware_controller()
except rospy.ROSInterruptException:
pass
def create_components(mqtt_broker):
# Get settings
availability_topic = env_bool('HA_AVAILABLE', False)
auto_discovery = env_bool('HA_AUTO', False)
# Setup registry
reg = ComponentRegistry()
state = MqttSharedTopic(mqtt_broker,
id_from_name("/{}".format(with_prefix('state'))))
reg.add_shared_topic(state)
# Setup standard config
standard_config = {
'mqtt': mqtt_broker,
'state_topic': state,
'availability_topic': availability_topic,
'auto_discovery': auto_discovery
}
# Create error sensor
errors = ErrorSensor(with_prefix('Errors'), **standard_config)
reg.add_component(errors)
# Create last update sensor
last_update = create_last_update_sensor(with_prefix('Last Update'),
**standard_config)
reg.add_component(last_update)
# Setup temperature sensors
func_limit = create_func_call_limiter()
ha_sensors = []
for s in create_temp_sensors():
logging.info("Found temp sensor: {}".format(s.get_id()))
# Wrap state function to catch errors
ha_sensors.append(
Sensor(with_prefix('Temp ' + env_name(s.get_id())),
'°C',
state_func=wrap_sensor(errors, func_limit, s),
**standard_config))
reg.add_component(ha_sensors)
# Setup temp sensor counter
reg.add_component(
Sensor(with_prefix('Temp Sensor Count'),
'',
state_func=lambda: len(ha_sensors),
state_formatter_func=lambda x: x,
icon='mdi:magnify-plus',
**standard_config))
# Create average temperature
avg = create_average_sensor(with_prefix('Temp Average'),
'°C',
ha_sensors,
icon='mdi:thermometer-lines',
**standard_config)
reg.add_component(avg)
# Create weighted average temperature
avg_w, weights = create_weighted_average_sensor(
with_prefix('Temp Average Weighted'),
'°C',
0,
100,
1,
50,
ha_sensors,
icon='mdi:thermometer-lines',
**standard_config)
reg.add_component(avg_w)
reg.add_component(weights, send_updates=False)
# Create SSR
ssr = Switch(with_prefix('SSR'),
state_change_func=create_ssr(errors),
**standard_config)
reg.add_component(ssr)
# Create PID sensor
pid_sensor = PidSensor()
s_p = Sensor(with_prefix('P'),
' ',
sensor_id=id_from_name(with_prefix('pid_p')),
state_func=lambda: pid_sensor.p,
**standard_config)
s_i = Sensor(with_prefix('I'),
' ',
sensor_id=id_from_name(with_prefix('pid_i')),
state_func=lambda: pid_sensor.i,
**standard_config)
s_d = Sensor(with_prefix('D'),
' ',
sensor_id=id_from_name(with_prefix('pid_d')),
state_func=lambda: pid_sensor.d,
**standard_config)
s_o = Sensor(with_prefix('Percent On'),
'%',
icon='mdi:power-socket-eu',
state_func=lambda: float(pid_sensor.control_percent),
**standard_config)
reg.add_component([s_p, s_i, s_d, s_o])
# Create PID controller
s_def = {
'mqtt': mqtt_broker,
'availability_topic': availability_topic,
'auto_discovery': auto_discovery,
'icon': 'mdi:cursor-move'
}
pid_controller = Pid(avg_w, ssr, pid_sensor)
reg.add_component(SettableSensor('P',
'',
0,
10,
0.1,
pid_controller.p,
lambda v: pid_controller.set_p(v),
sensor_id=id_from_name(
with_prefix('pid_p_gain')),
**s_def),
send_updates=False)
reg.add_component(SettableSensor('I',
'',
0,
10,
0.1,
pid_controller.i,
lambda v: pid_controller.set_i(v),
sensor_id=id_from_name(
with_prefix('pid_i_gain')),
**s_def),
send_updates=False)
reg.add_component(SettableSensor('D',
'',
0,
10,
0.1,
pid_controller.d,
lambda v: pid_controller.set_d(v),
sensor_id=id_from_name(
with_prefix('pid_d_gain')),
**s_def),
send_updates=False)
reg.add_component(SettableSensor(
'Out Limit',
'',
0,
10,
1,
pid_controller.output_limit,
lambda v: pid_controller.set_output_limit(v),
sensor_id=id_from_name(with_prefix('pid_output_limit')),
**s_def),
send_updates=False)
reg.add_component(SettableSensor(
'Time',
'',
0,
10,
1,
pid_controller.sample_time,
lambda v: pid_controller.set_sample_time(v),
sensor_id=id_from_name(with_prefix('pid_sample_time')),
**s_def),
send_updates=False)
# Create Time On Target sensor
tat = TimeOnTarget(pid_controller)
reg.add_component(
Sensor(with_prefix('Time On Target'),
'',
state_func=errors.wrap_function(tat),
icon='mdi:clock',
device_class='timestamp',
state_formatter_func=lambda s: s,
**standard_config))
# Create HA climate
climate = Climate(
with_prefix(),
avg_w,
state_change_func=lambda mode, target: pid.state_change(mode, target),
mqtt=mqtt_broker,
availability_topic=availability_topic,
auto_discovery=auto_discovery,
# state_topic=state
)
reg.add_component(climate)
if not SIMULATE:
# RPI temp sensor
reg.add_component(
Sensor(with_prefix('Temp CPU'),
'°C',
state_func=cpu_temp,
**standard_config))
cfg = reg.create_config()
file = '/tmp/ha-cfg.yml'
logging.info("Writing HA config to file: {}".format(file))
with open(file, 'w') as f:
f.write(cfg)
if env_bool('HA_PRINT_CONFIG', False):
logging.info("HA config:\n{}".format(cfg))
return func_limit, pid_controller, reg, climate, errors
