四十行代码实现robomaster（青少年）自瞄

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)