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