pwm = PWM() # arm the motors pwm.arm() # initialize Pressure Sensor pressure = PressureSensor() # calibrate Pressure Sensor pressure.calibrate() imu = IMU() sleep(1) depthPID = PID(p=145, i=0.1, d=1) depthPID.target = 0.9 headingPID = PID(p=0.75, i=0.01, d=0) init_heading, _, _ = imu.get_required(0) headingPID.target = init_heading - 15 count = 0 start = time() while True: try: if not controller.connected(): depth = pressure.get_depth() heading, roll, accy = imu.get_required( ref_heading=headingPID.target) depthCorrection = -int(depthPID(feedback=depth)) headingCorrection = -int(headingPID(feedback=heading)) if depth > 0.1:
val = drone.get_param(params[i]) if val is not None: print "found existing value for param", params[i], val i = val else: print "creating new param", params[i], i drone.create_param(params[i], i) v_s_switch = True # Initialize two separate PID controllers for two axis. # Controller's target is to bring delta angle between image center and object's centroid to zero. # Controller is not aware of current position of gimbal. # Since position feedback from gimbal is not available run controller at lower speed than the gimbal can move. Then assume that current position of gimbal is last commanded state. pidc_X = PID(p=kp, i=ki, d=kd) pidc_X.target = 0 pidc_Y = PID(p=kp, i=ki, d=kd) pidc_Y.target = 0 # Initialize gimbal to (0,0,0) angles. drone.gimbal_set(0.0, 0.0, 0.0) time.sleep(5.0) # initialize a rospy.Timer to force controller rate. control_loop = rospy.Timer(period=rospy.Duration(loop_rate), callback=control_loop_callback, oneshot=False) # subscribe to param update msg param__update_sub = rospy.Subscriber( "/" + drone.namespace + "/parameter_updated", String, param_update_callback)