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
