import drive
import motor
import competition_control
import remote_controller
import time

距离 = None
前后 = None
speed = None

def _E5_89_8D(距离, 前后):
  global speed
  competition_control.checkpoint()
  competition_control.checkpoint()
  competition_control.checkpoint()
  drive.move_cm(3, 前后, 距离)

def _E5_B7_A6_E8_BD_AC():
  global 距离, 前后, speed
  competition_control.checkpoint()
  competition_control.checkpoint()
  competition_control.checkpoint()
  drive.rotate((-90) * 2, 50)


competition_control.checkpoint()
drive.set_pid(0.7, 0.008, 0.002)
competition_control.checkpoint()
吸轮马达 = motor.smart_motor(1, openloop_remap_cut=int(10), direction=True)
competition_control.checkpoint()
speed = 0
competition_control.checkpoint()
with competition_control.Session(remote_controller.START, 15, remote_controller, ((drive.stop_rpm, (0,)), (吸轮马达.stop, ()), (吸轮马达.stop, (True,)),), scan_remote=False, manual_interval_ms=0, auto_enabled=True) as competition_session:
  while competition_session.poll():
    if competition_session.is_auto():
      with competition_session.auto_phase():
        competition_control.checkpoint()
        _E5_89_8D(50, 50)
        competition_control.checkpoint()
        time.sleep_ms(int(4000))
        competition_control.checkpoint()
        _E5_B7_A6_E8_BD_AC()
        competition_control.checkpoint()
        time.sleep_ms(int(1000))
        competition_control.checkpoint()
        _E5_89_8D(20, 50)
        competition_control.checkpoint()
        time.sleep_ms(int(2000))
        competition_control.checkpoint()
        _E5_89_8D(20, -50)
        competition_control.checkpoint()
        time.sleep_ms(int(2000))
    elif competition_session.is_manual():
      competition_control.checkpoint()
      if remote_controller.get_button(remote_controller.A):
        competition_control.checkpoint()
        speed = 0
      elif remote_controller.get_button(remote_controller.M):
        competition_control.checkpoint()
        speed = 1
      competition_control.checkpoint()
      if remote_controller.get_button(remote_controller.UP):
        competition_control.checkpoint()
        drive.drive_rpm(100, 100)
      elif remote_controller.get_button(remote_controller.DOWN):
        competition_control.checkpoint()
        drive.drive_rpm(-100, -100)
      elif remote_controller.get_button(remote_controller.LEFT):
        competition_control.checkpoint()
        drive.drive_rpm(-100, 100)
      elif remote_controller.get_button(remote_controller.RIGHT):
        competition_control.checkpoint()
        drive.drive_rpm(100, -100)
      elif speed == 0:
        competition_control.checkpoint()
        if remote_controller.get_axis(remote_controller.LEFT_Y) - remote_controller.get_axis(remote_controller.RIGHT_Y) > 40 or remote_controller.get_axis(remote_controller.RIGHT_Y) - remote_controller.get_axis(remote_controller.LEFT_Y) > 40:
          competition_control.checkpoint()
          drive.drive_rpm(remote_controller.get_axis(remote_controller.LEFT_Y) * 0.8, remote_controller.get_axis(remote_controller.RIGHT_Y) * 0.8)
        else:
          competition_control.checkpoint()
          drive.drive_rpm(remote_controller.get_axis(remote_controller.LEFT_Y) * 1, remote_controller.get_axis(remote_controller.RIGHT_Y) * 1)
      elif speed == 1:
        competition_control.checkpoint()
        if remote_controller.get_axis(remote_controller.LEFT_Y) - remote_controller.get_axis(remote_controller.RIGHT_Y) > 40 or remote_controller.get_axis(remote_controller.RIGHT_Y) - remote_controller.get_axis(remote_controller.LEFT_Y) > 40:
          competition_control.checkpoint()
          drive.control_motor(1, remote_controller.get_axis(remote_controller.LEFT_Y) * 0.66, soft_break=False)
          competition_control.checkpoint()
          drive.control_motor(2, remote_controller.get_axis(remote_controller.RIGHT_Y) * 0.66, soft_break=False)
        else:
          competition_control.checkpoint()
          drive.control_motor(1, remote_controller.get_axis(remote_controller.LEFT_Y) * 0.85, soft_break=False)
          competition_control.checkpoint()
          drive.control_motor(2, remote_controller.get_axis(remote_controller.RIGHT_Y) * 0.85, soft_break=False)
      competition_control.checkpoint()
      if remote_controller.get_button(remote_controller.R1):
        competition_control.checkpoint()
        吸轮马达.openloop_turn(int(80))
      elif remote_controller.get_button(remote_controller.R2):
        competition_control.checkpoint()
        吸轮马达.openloop_turn(int(-80))
      else:
        competition_control.checkpoint()
        吸轮马达.openloop_turn(int(0))