pid_D = PIDCtrl()
pid_Pitch = PIDCtrl()
pid_pitch = PIDCtrl()
pid_yaw = PIDCtrl()
variable_X = 0
variable_Y = 0
variable_Makerlist = 0
variable_Maker = 0
variable_out = 0
list_Maker = RmList()
list_out = RmList()
def start():
global variable_X
global variable_Y
global variable_Makerlist
global variable_Maker
global variable_out
global list_Maker
global list_out
global pid_D
global pid_Pitch
global pid_pitch
global pid_yaw
media_ctrl.exposure_value_update(rm_define.exposure_value_small)
vision_ctrl.set_marker_detection_distance(3)
vision_ctrl.enable_detection(rm_define.vision_detection_marker)
robot_ctrl.set_mode(rm_define.robot_mode_free)
pid_yaw.set_ctrl_params(100,0.5,0.6)
pid_Pitch.set_ctrl_params(120,0.9,0.8)
while True:
list_Maker=RmList(vision_ctrl.get_marker_detection_info())
if list_Maker[1] == 1:
variable_X = list_Maker[3]
variable_Y = list_Maker[4]
pid_Pitch.set_error(variable_X - 0.52)
pid_yaw.set_error(0.49 - variable_Y)
variable_out = pid_Pitch.get_output()
gimbal_ctrl.rotate_with_speed(pid_Pitch.get_output() * 1,pid_yaw.get_output())
if abs(variable_X - 0.52) < 0.05 and abs(0.49 - variable_Y) < 0.05:
gun_ctrl.set_fire_count(2)
gun_ctrl.fire_once()
time.sleep(1)
else:
gimbal_ctrl.rotate_with_speed(0,0)