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 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): 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 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 __init__(self): # Velocity PID object setup self.vel_pid_output_a = VelocityPidOutput() self.vel_pid_output_b = VelocityPidOutput() self.vel_pid_output_c = VelocityPidOutput() self.vel_pid_a = Pid(self.vel_pid_output_a, Robot.VEL_P, Robot.VEL_I, Robot.VEL_D, Robot.VEL_F) self.vel_pid_b = Pid(self.vel_pid_output_b, Robot.VEL_P, Robot.VEL_I, Robot.VEL_D, Robot.VEL_F) self.vel_pid_c = Pid(self.vel_pid_output_c, Robot.VEL_P, Robot.VEL_I, Robot.VEL_D, Robot.VEL_F) # Qep encoder object setup self.qep_a = Qep(Robot.MOTOR_A_QEP) self.qep_b = Qep(Robot.MOTOR_B_QEP) self.qep_c = Qep(Robot.MOTOR_C_QEP) # Pwm object setup self.pwm_a = Pwm(Robot.MOTOR_A_PWM) self.pwm_b = Pwm(Robot.MOTOR_B_PWM) self.pwm_c = Pwm(Robot.MOTOR_C_PWM) self.pwms = [self.pwm_a, self.pwm_b, self.pwm_c] # Create MotorController objects self.motor_a = MotorController(self.pwm_a, self.vel_pid_a, self.vel_pid_output_a, self.qep_a, self.VEL_PID_ENABLED) self.motor_b = MotorController(self.pwm_b, self.vel_pid_b, self.vel_pid_output_b, self.qep_b, self.VEL_PID_ENABLED) self.motor_c = MotorController(self.pwm_c, self.vel_pid_c, self.vel_pid_output_c, self.qep_c, self.VEL_PID_ENABLED) self.motors = [self.motor_a, self.motor_b, self.motor_c] # set up pid self.yaw_pid_output = YawPidOutput() self.yaw_pid = Pid(self.yaw_pid_output, Robot.YAW_P, Robot.YAW_I, Robot.YAW_D, izone=1.0) # initialise mpu/imu server module self.mpu = Mpu() self.yaw_pid_thread = YawPidThread(self.yaw_pid, self.mpu) self.current_command = Robot.INIT_COMMAND # pid *enabled* by default self.yaw_pid_enabled = False # pid not in *control* by default, this is automatically set by drive self.pid_in_control = False self.field_centered = True self.enabled = False self.interrupted = False self.last_input_time = time.time()
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 __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}
class fControl: 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 tune(self, controller, P, I, D): if controller == "x": self.xPid.tune(P, I, D) elif controller == "y": self.yPid.tune(P, I, D) elif controller == "z": self.zPid.tune(P, I, D) def setSpeed(self, direction, value): self.speeds[direction] = value def startHover(self, pos): self.setPoints["x"] = pos.x self.setPoints["y"] = pos.y self.setPoints["z"] = pos.z self.state = "hover" def update(self, state, pos): self.state = state self.pos = pos if self.state == "landed": return # Put setpoints over/under current to move in z direction if self.state == "moveUP": self.setPoints["z"] = pos.z - self.speeds["throttle"] elif self.state == "moveDown": self.setPoints["z"] = pos.z + self.speeds["throttle"] #Feed forward control if position info is invalid if pos.zValid == False: if state == "moveUp": self.output["throttle"] += throttleIncrement elif state == "moveDown": self.output["throttle"] -= throttleIncrement #run altitude controller elif pos.zValid == True: self.zPid.set(self.setPoints["z"]) self.zPid.step(pidDt, pos.z) self.output["throttle"] -= self.zPid.get( ) #negative because of NED # Run X/Y-controllers in hover mode if self.state == "hover": if pos.xValid: self.xPid.set(self.setPoints["x"]) self.xPid.step(pidDt, pos.x) self.output["pitch"] = midOutput - self.xPid.get() #negative because low elevator is forward else: self.output["pitch"] = midOutput if pos.yValid: self.yPid.set(self.setPoints["y"]) self.yPid.step(pidDt, pos.y) self.output["roll"] = midOutput + self.yPid.get() else: self.output["roll"] = midOutput return # Moving states. Feed forward attitude setpoints to FC if self.state == "turnLeft": self.output["yaw"] = midOutput + self.speeds["yaw"] elif self.state == "turnRight": self.output["yaw"] = midOutput - self.speeds["yaw"] elif self.state == "moveRight": self.output["roll"] = midOutput + self.speeds["roll"] elif self.state == "moveLeft": self.output["roll"] = midOutput - self.speeds["roll"] elif self.state == "moveForward": self.output["pitch"] = midOutput - self.speeds["pitch"] elif self.state == "moveBackward": self.output["pitch"] = midOutput + self.speeds["pitch"]
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
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()
def main(): rospy.init_node('pid_controller', anonymous=True) long_control = Pid() # lat_control.angle_pred() rospy.spin()
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)
logging.basicConfig(filename=OB.LOG_BASE_DIR + OB.LOG_FILE_NAME, format='%(asctime)s %(levelname)s %(message)s', level=logging.DEBUG) logging.info('%s', 'Start!') # Create instances. TANK = TempSensor(S, OB.TANK_TEMP_SENSOR_ID) BOILER = TempSensor(S, OB.BOILER_TEMP_SENSOR_ID) # FALLBACK_TEMP = TempSensor(S, "286B075005000099") FIRE = FireSensor(S, OB.FIRE_TEMP_SENSOR_ANALOG_PIN) FAN = AnalogOut(S, OB.FAN_CONTACTOR_PIN, OB.FAN_ANALOG_PIN) SOND = SerialLambda(S, OB.LAMBDA_SENSOR_CONTACTOR_PIN, OB.LAMBDA_SENSOR_TYPE, OB.LAMBDA_SENSOR_PORT, OB.LAMBDA_SENSOR_BAUD, int(OB.LAMBDA_SENSOR_SYNC_HEADER_ATTEMPT)) SCREW = Screw(S, OB.SCREW_CONTACTOR_PIN) HMI = Hmi(S, OB.BUTTON_CONTACTOR_PIN) # PID p=Pid(10.0,0.0,0) p.setPoint(30) HMI.activate_contactor() # Activate HMI buttons t1 = threading.Thread(target=startWebbServer, args=(q,)) t1.start() # START WEBBSERVER IN NEW THRED logging.info('%s', "Enter main loop") while True: q.put(TANK.get_temp(), BOILER.get_temp(), FIRE.get_temp(), int(FAN.get_rpm()), S.input_get_value(OB.BUTTON_AUTO_MAN), TIME_LEFT_OF_BLOCK_TIMER)# UPPDATE VALUE FOR GUI if not TIMESTAMP_CFG == time.ctime(os.path.getmtime(CFG)): # RELOAD CONFIG IF TIMESTAMP CHANGED TIMESTAMP_CFG = time.ctime(os.path.getmtime(CFG)) OB.readConfigFile() logging.info('%s', "Reload config file")
print ("grab_title:", sys.exc_info()[0]) return "" def rss_send(self, to): logging.info('rss_send') rss = rssPull(['http://rss.orf.at/news.xml', 'http://heroicdebugging.biz/feeds/all.atom.xml']) #should be constructed from configparser rss_items = rss.getNewItems() for item in rss_items: msg_string = 'Title: {0}\nLink: {1}'.format(item['title'],item['link']) self.send_message(mto=to, mbody=msg_string, mtype='chat') if __name__ == '__main__': logging.basicConfig(level=logging.DEBUG, format='%(levelname)-8s %(message)s') pidinstance = Pid('./.kuhbot_pid') if(pidinstance.read()==1): sys.exit(1) if(pidinstance.write()==1): sys.exit(1) #configparser try: config = configparser.ConfigParser() config_file = 'kuhbot_config.txt' config['LOGIN'] = { 'jid' : '', 'password' : '', 'nick' : '', } config['ROOMS'] = {}
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)
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
class Robot(object): VEL_P = 0.0 VEL_I = 0.0 VEL_D = 0.0 VEL_F = 1.0 YAW_P = 0.0 YAW_I = 0 YAW_D = 0 # Pwm and Qep ids for motor controllers a, b and c MOTOR_A_PWM = "PWM0B-29" MOTOR_A_QEP = "QEP0" MOTOR_B_PWM = "PWM1A-36" MOTOR_B_QEP = "QEP1" MOTOR_C_PWM = "PWM2A-45" MOTOR_C_QEP = "QEP2" INIT_COMMAND = "OmniDrive" VEL_PID_ENABLED = True ROBOT_MAX_X_SPEED = math.sin(math.radians(60)) # approx 0.87 (root 2 over 3) ROBOT_MAX_Y_SPEED = math.cos(math.radians(60)) # 0.50 # the speed of rotation at which we shut down the pid so it does not oscillate # as you set the set point while still rotating YAW_MOMENTUM_THRESHOLD = math.radians(10.0) # degrees per second TIME_TO_AUTO_DISABLE = 10 # seconds till we automatically disable AUTO_DISABLE_THRESHOLD = 0.05 def __init__(self): # Velocity PID object setup self.vel_pid_output_a = VelocityPidOutput() self.vel_pid_output_b = VelocityPidOutput() self.vel_pid_output_c = VelocityPidOutput() self.vel_pid_a = Pid(self.vel_pid_output_a, Robot.VEL_P, Robot.VEL_I, Robot.VEL_D, Robot.VEL_F) self.vel_pid_b = Pid(self.vel_pid_output_b, Robot.VEL_P, Robot.VEL_I, Robot.VEL_D, Robot.VEL_F) self.vel_pid_c = Pid(self.vel_pid_output_c, Robot.VEL_P, Robot.VEL_I, Robot.VEL_D, Robot.VEL_F) # Qep encoder object setup self.qep_a = Qep(Robot.MOTOR_A_QEP) self.qep_b = Qep(Robot.MOTOR_B_QEP) self.qep_c = Qep(Robot.MOTOR_C_QEP) # Pwm object setup self.pwm_a = Pwm(Robot.MOTOR_A_PWM) self.pwm_b = Pwm(Robot.MOTOR_B_PWM) self.pwm_c = Pwm(Robot.MOTOR_C_PWM) self.pwms = [self.pwm_a, self.pwm_b, self.pwm_c] # Create MotorController objects self.motor_a = MotorController(self.pwm_a, self.vel_pid_a, self.vel_pid_output_a, self.qep_a, self.VEL_PID_ENABLED) self.motor_b = MotorController(self.pwm_b, self.vel_pid_b, self.vel_pid_output_b, self.qep_b, self.VEL_PID_ENABLED) self.motor_c = MotorController(self.pwm_c, self.vel_pid_c, self.vel_pid_output_c, self.qep_c, self.VEL_PID_ENABLED) self.motors = [self.motor_a, self.motor_b, self.motor_c] # set up pid self.yaw_pid_output = YawPidOutput() self.yaw_pid = Pid(self.yaw_pid_output, Robot.YAW_P, Robot.YAW_I, Robot.YAW_D, izone=1.0) # initialise mpu/imu server module self.mpu = Mpu() self.yaw_pid_thread = YawPidThread(self.yaw_pid, self.mpu) self.current_command = Robot.INIT_COMMAND # pid *enabled* by default self.yaw_pid_enabled = False # pid not in *control* by default, this is automatically set by drive self.pid_in_control = False self.field_centered = True self.enabled = False self.interrupted = False self.last_input_time = time.time() def drive(self, vX, vY, vZ, throttle): if not self.enabled: self.last_input_time = time.time() for motor in self.motors: motor.set_speed(0.0) for p in self.pwms: p.set_speed(0.0) p.pwm_off() return else: for p in self.pwms: p.pwm_on() if math.fabs(vX) <= self.AUTO_DISABLE_THRESHOLD and math.fabs(vY) <= self.AUTO_DISABLE_THRESHOLD and math.fabs(vZ) <= self.AUTO_DISABLE_THRESHOLD: if time.time() - self.last_input_time > self.TIME_TO_AUTO_DISABLE: self.enabled = False else: self.last_input_time = time.time() vPID = 0.0 if self.field_centered: vX, vY = self.field_orient(vX, vY, self.mpu.get_euler()[0]) # Drive equations that translate vX, vY and vZ into commands to be sent to the motors # front motor mA = -(((0.0*vX) + (vY * 1.0))/2.0 + vZ/3.0) # bottom left motor mB = (((-vX * math.sin(math.radians(60))) + (-vY / 2.0))/2.0 + vZ/3.0) # bottom right motor mC = (((vX * math.sin(math.radians(60))) + (-vY / 2.0))/2.0 + vZ/3.0) motor_input = [mA, mB, mC] if self.yaw_pid_enabled: if not vZ == 0: # spinning under command -> no PID self.pid_in_control = False elif math.fabs(self.mpu.get_gyro()[2]) < Robot.YAW_MOMENTUM_THRESHOLD: # momentum is less than the threshold and we are not under command # -> PID can now take control self.pid_in_control = True if self.pid_in_control: vPID = self.yaw_pid_output.value else: heading = self.mpu.get_euler()[0] # yaw self.yaw_pid.set_set_point(heading) # zero the correction value so that the next time the code runs the existing # correction value will not still be there self.yaw_pid_output.value = 0.0 max = 1.0 # find the maximum motor speed for i in range(3): if math.fabs(motor_input[i]) > max: max = abs(motor_input[i]) # scale between -1 and 1 for i in range(3): motor_input[i] /= max # multiply by throttle for i in range(3): motor_input[i] *= throttle if self.yaw_pid_enabled: max = 1 for i in range(3): motor_input[i] -= vPID if math.fabs(motor_input[i]) > max: max = math.fabs(motor_input[i]) for i in range(3): motor_input[i] /= max # set the speeds of the motors for i in range(3): self.motors[i].set_speed(motor_input[i]) def field_orient(self, vX, vY, yaw_angle): oriented_vx = vX*math.cos(yaw_angle)+vY*math.sin(yaw_angle) oriented_vy = -vX*math.sin(yaw_angle)+vY*math.cos(yaw_angle) return oriented_vx, oriented_vy