import supply_voltage #导入它，开启电量指示呼吸灯功能
import motor
import sensor
import controller
import tool

down = None
功率测试 = None
底盘极速 = None
lift_btn_timer = None
auto_lift = None


down = 0
功率测试 = 1
底盘极速 = 80
抬升1 = motor.smart_motor(3, direction=False)
抬升1.set_soft_break(True)
抬升2 = motor.smart_motor(4, direction=True)
抬升2.set_soft_break(True)
角度传感器1 = sensor.angle_sensor(1)
while True:
    if controller.get_button(controller.L1):
        lift_btn_timer = lift_btn_timer + 1
        抬升1.openloop_turn(100)
        抬升2.openloop_turn(100)
        if lift_btn_timer < 20:
            if 角度传感器1.get_value() < 19:
                auto_lift = 1
            else:
                auto_lift = 0
        else:
            auto_lift = 0
        down = 2
    elif controller.get_button(controller.L2):
        抬升1.openloop_turn((-30))
        抬升2.openloop_turn((-30))
        auto_lift = 0
        down = 3
    elif controller.get_button(controller.P):
        抬升1.openloop_turn((-100))
        抬升2.openloop_turn((-100))
        down = 1
        auto_lift = 0
    elif controller.get_button(controller.ROCKET):
        down = 0
        auto_lift = 0
    elif auto_lift == 1:
        lift_btn_timer = 0
        抬升1.openloop_turn((5 * (20 - 角度传感器1.get_value())))
        抬升2.openloop_turn((5 * (20 - 角度传感器1.get_value())))
    else:
        lift_btn_timer = 0
        if down == 1:
            抬升1.openloop_turn((-40))
            抬升2.openloop_turn((-40))
        elif down == 2:
            抬升1.openloop_turn(1)
            抬升2.openloop_turn(1)
        elif down == 3:
            抬升1.openloop_turn((-1))
            抬升2.openloop_turn((-1))
        else:
            抬升1.openloop_turn(0)
            抬升2.openloop_turn(0)
    tool.my_print(角度传感器1.get_value())