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

速度 = None
时间 = None
角度 = None
move = None
turn = None
当前时间 = None
gyro = None

def drive(move, turn):
    global 速度, 时间, 角度, 当前时间, gyro
    我的数字电机1.openloop_turn((move - turn))
    我的数字电机2.openloop_turn((move + turn))

def move2(速度, 时间, 角度):
    global move, turn, 当前时间, gyro
    姿态传感器1.set_attitude_zero()
    我的PID1 = pid.PID(p=0.9, i=0.2, d=0.09, imax=100)
    当前时间 = 0
    while 时间 >= 当前时间:
        gyro = (姿态传感器1.get_attitude()[2])
        drive(速度, 我的PID1.get_pid((角度 - gyro), 1))
        time.sleep(0.1)
        当前时间 = 当前时间 + 100
    drive(0, 0)

def turn2(时间, 角度):
    global 速度, move, turn, 当前时间, gyro
    姿态传感器1.set_attitude_zero()
    我的PID2 = pid.PID(p=1, i=0.2, d=0.1, imax=100)
    当前时间 = 0
    while 时间 >= 当前时间:
        gyro = (姿态传感器1.get_attitude()[2])
        drive(0, 我的PID2.get_pid((角度 - gyro), 1))
        time.sleep(0.1)
        当前时间 = 当前时间 + 100
    drive(0, 0)


姿态传感器1 = sensor.attitude('A', 'B')
我的数字电机1 = motor.smart_motor(1, direction=True)
我的数字电机1.set_soft_break(True)
我的数字电机2 = motor.smart_motor(2, direction=False)
我的数字电机2.set_soft_break(True)
姿态传感器1.set_attitude_zero()
for count in range(1):
    pass