def blocked_yaw(self, yaw=120): throttle_pid = pid_controller.PID(P=1.0, I=0.0, D=0.5, Integrator_max=0.5, Integrator_min=0.0) throttle_pid.setPoint(self.current_alt) "attempting to maintain alt: ", self.current_alt time.sleep(2) while True: att_pub = SP.get_pub_attitude_pose(queue_size=10) thd_pub = SP.get_pub_attitude_throttle(queue_size=10) #while not rospy.is_shutdown(): pose = SP.PoseStamped(header=SP.Header(stamp=rospy.get_rostime())) q = quaternion_from_euler(0, 0, 90) pose.pose.orientation = SP.Quaternion(*q) throt = throttle_pid.update(self.current_alt) #if throt > 0.6: # throt = 0.6 #if throt < 0.4: # throt = 0.6 if self.attitude_publish or True: att_pub.publish(pose) thd_pub.publish(data=throt) print "pose orientation: ", throt
def velocity_init(self): self.vx = 0.0 self.vy = 0.0 self.vz = 0.0 self.yaw = 0.0 self.velocity_publish = False self.use_pid = True self.pid_alt = pid_controller.PID() self.pid_alt.setPoint(1.0) # publisher for mavros/setpoint_position/local self.pub_vel = SP.get_pub_velocity_cmd_vel(queue_size=10) # subscriber for mavros/local_position/local self.sub = rospy.Subscriber( mavros.get_topic('local_position', 'local'), SP.PoseStamped, self.temp) try: thread.start_new_thread(self.navigate, ()) except: fault("Error: Unable to start thread") # TODO(simon): Clean this up. self.done = False self.done_evt = threading.Event()
def __init__(self): rospy.init_node('gps_goto') mavros.set_namespace( ) # initialize mavros module with default namespace self.x = 0.0 self.y = 0.0 self.z = 0.0 self.current_lat = 0.0 self.current_lon = 0.0 self.current_alt = 0.0 self.throttle_update = 0.0 self.attitude_publish = True self.pid_throttle = pid_controller.PID(P=0.8, I=0.8, D=0.8, Integrator_max=1, Integrator_min=0) self.pid_throttle.setPoint(10) self.setHome = False self.home_lat = 0.0 self.home_lon = 0.0 self.home_alt = 0.0 self.publish = True self.velocity_init()
def __init__(self): # Connect to the gripper using default settings self.GPR = gp.Gripper() #self.CS = cs.NP_05B() self._pos_file = os.path.join(this_dir, "POS", "chwp_control_positions.txt") self._read_pos() self._log = lg.Logging() self.pid = pc.PID() return
def __init__(self, copter_id = "1"): self.copter_id = copter_id mavros_string = "/mavros/copter"+copter_id #rospy.init_node('velocity_goto_'+copter_id) mavros.set_namespace(mavros_string) # initialize mavros module with default namespace self.pid_alt = pid_controller.PID() self.mavros_string = mavros_string self.final_alt = 0.0 self.final_pos_x = 0.0 self.final_pos_y = 0.0 self.final_vel = 0.0 self.cur_rad = 0.0 self.cur_alt = 0.0 self.cur_pos_x = 0.0 self.cur_pos_y = 0.0 self.cur_vel = 0.0 self.vx = 0.0 self.vy = 0.0 self.vz = 0.0 self.pose_open = [] self.alt_control = True self.override_nav = False self.reached = True self.done = False self.last_sign_dist = 0.0 # for local button handling self.click = " " self.button_sub = rospy.Subscriber("abpause_buttons", std_msgs.msg.String, self.handle_buttons) # publisher for mavros/copter*/setpoint_position/local self.pub_vel = SP.get_pub_velocity_cmd_vel(queue_size=10) # subscriber for mavros/copter*/local_position/local self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'local'), SP.PoseStamped, self.temp)
def velocity_gps_goto(self, lat, lon, alt): pid_lat = pid_controller.PID() self.pid_alt.setPoint(alt) self.use_pid = True pid_lat.setPoint(lat) self.set_publish(False) self.set_velocity_publish(True) while abs(self.current_lat - lat) > 0.2: vel_x = pid_lat.update(self.current_lat) if vel_x > 5: vel_x = 5 if vel_x < -5: vel_x = -5 #self.publish_orientation() self.yaw = 67 self.set_velocity(vel_x, 0, 0) print "ABS current lat min lat: ", abs(self.current_lat - lat) #print "Current lat, target lat: ", self.current_lat, " ", lat self.set_publish(False)
def normalizeDutyCycle(x): if x < 0: return 0 elif x < minDC: return 0 elif x > 100: return maxDC else: #Normalize to nearest 5 #return int(round(x/5)*5) return int(round(x)) if __name__ == "__main__": #pc = pid_controller.PID(P=6, I=0.02, D=0.0) pc = pid_controller.PID(P=6, I=0.02, D=0.0) target_temp = 225 pc.SetPoint = target_temp max_temp = 240 start_temp = 100 num_temps = 100 input_temps = [ start_temp + (x * (max_temp - start_temp) / num_temps) for x in xrange(num_temps) ] input_temps += [target_temp] * num_temps rand_input_temps = [generateRandomTemp(x) for x in input_temps] for x in rand_input_temps: pc.update(x) dc = normalizeDutyCycle(pc.output)
# running = False surface_velocity_frame = vessel.surface_velocity_reference_frame surface_frame = vessel.surface_reference_frame orbit_frame = vessel.orbit.body.reference_frame velocity_surface = connection.add_stream(connection.space_center.transform_velocity,vessel.position(orbit_frame),vessel.velocity(orbit_frame),orbit_frame,surface_frame) altitude = connection.add_stream(getattr, vessel.flight(), 'mean_altitude') surface_altitude = connection.add_stream(getattr, vessel.flight(), 'surface_altitude') latitude = connection.add_stream(getattr,vessel.flight(),'latitude') longitude = connection.add_stream(getattr,vessel.flight(),'longitude') stage_1_resources = vessel.resources_in_decouple_stage(stage=1, cumulative=False) launcher_liquid_fuel = connection.add_stream(stage_1_resources.amount, 'LiquidFuel') altitude_pid = pid_controller.PID(0.2,0.0,0.0,0.0,0.0) vertical_speed_pid = pid_controller.PID(0.2,2.0,0.0,1.0,0.0) speed_pid = pid_controller.PID(0.2,0.0,0.0,1.0,0.0) horizontal_speed_pid = pid_controller.PID(2.0,0.0,0.0,20.0,-20.0) pos_latitude_pid = pid_controller.PID(750,0,0.0,0,0) pos_longitude_pid = pid_controller.PID(750,0,0.0,0,0) altitude_target = 500 vertical_speed_target = 0 state = "INIT" loop_time = 0.1 launch_site_altitude_ASL = altitude() launch_site_latitude = latitude() launch_site_longitude = longitude() kerbin = connection.space_center.bodies['Kerbin'] launch_site_radius = 10/(2*math.pi*kerbin.equatorial_radius)*360.0
enableAngularPIDControl = rospy.get_param('/simple_differential_controller/enableAngularPID') print("Setting Initial Linear Kp:%0.2f" % linearPID_Kp) print("Setting Initial Linear Ki: %0.2f" % linearPID_Ki) print("Setting Initial Linear Kd: %0.2f" % linearPID_Kd) print("Setting Initial Linear windup: %0.2f" % linearPID_windup) print("Setting Initial Angular Kp:%0.2f" % angularPID_Kp) print("Setting Initial Angular Ki: %0.2f" % angularPID_Ki) print("Setting Initial Angular Kd: %0.2f" % angularPID_Kd) print("Setting Initial Angular windup: %0.2f" % angularPID_windup) print("Enable angular PID: {}".format(enableAngularPIDControl)) #linear_pid = pid_controller.PID( Kp=0.3, Ki=.05, Kd=0.0, windup_limit=10 ) #angular_pid = pid_controller.PID( Kp=100, Ki=10 ,Kd=0.0, windup_limit=50 ) linearL_pid = pid_controller.PID( Kp=linearPID_Kp, Ki=linearPID_Ki, Kd=linearPID_Kd, windup_limit=linearPID_windup ) angular_pid = pid_controller.PID( Kp=angularPID_Kp, Ki=angularPID_Ki ,Kd=angularPID_Kd, windup_limit=angularPID_windup ) linearR_pid = pid_controller.PID( Kp=linearPID_Kp, Ki=linearPID_Ki, Kd=linearPID_Kd, windup_limit=linearPID_windup ) def cmd_callback(data): rospy.logdebug(data._connection_header['topic']+' from '+data._connection_header['callerid']) global last_command global last_command_time last_command = data last_command_time = rospy.Time.now() def odom_callback(data): global last_command_time global last_odom_time global linearPID_Kp
this_dir = os.path.dirname(__file__) sys.path.append(os.path.join(this_dir, '..', 'config')) sys.path.append(this_dir) import pid_config import pid_controller # Change this to the directory which holds slowdaq slowdaq_dir = '' sys.path.append(slowdaq_dir) from slowdaq3.slowdaq.pb2 import Publisher # Instantiates a publisher instance for the PID controller pub = Publisher('pid_info', pid_config.aggregator_ip, pid_config.aggregator_port) pid = pid_controller.PID() # Every 10 seconds send the CHWP frequency to slowdaq i = 0 def main(): while True: pub.serve() pid.get_freq() data = pub.pack({'chwp_freq': pid.cur_freq}) pub.queue(data) i += 1 if not i % 2:
from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry from marine_msgs.msg import DifferentialDrive from std_msgs.msg import Float32 import datetime last_command = None last_command_time = None differential_pub = None max_linear_speed = 30.0 max_angular_speed = 0.39 linear_pid = pid_controller.PID(Kp=0.25, Ki=0.2, Kd=0.01, windup_limit=0.5) angular_pid = pid_controller.PID(Kp=3, Ki=0.4, Kd=0.0, windup_limit=5) linear_pid.lower_limit = -1.0 linear_pid.upper_limit = 1.0 angular_pid.lower_limit = -1.0 angular_pid.upper_limit = 1.0 def cmd_callback(data): global last_command global last_command_time last_command = data last_command_time = datetime.datetime.now()
offset = 340 hat.set_pwm(12, 0, 340) hat.set_pwm(13, 0, 340) time.sleep(1) rospy.init_node("depth_control") control_effort_pub = rospy.Publisher("control_effort", Float64, queue_size=5) setpoint_pub = rospy.Publisher("setpoint", Float64, queue_size=5) state_pub = rospy.Publisher("state", Float64, queue_size=5) pid_depth = pid_controller.PID(kp=1000, ki=1000, kd=800, wind_up=20, upper_limit=100, lower_limit=-100) setpoint = 0 state = 0 pid_depth.enable = True def run(): global setpoint while not rospy.is_shutdown(): sensor.read() state = sensor.depth() - 0.35 pwm = pid_depth.update(setpoint=setpoint, state=state) pwm = -pwm