text: from external.API_interface.TLF_API.component.Class_Pose2D import Pose2D
  from gpiozero.pins.rpigpio import RPiGPIOFactory
  from gpiozero import Servo, Device, LED
  from module.servo_cote import Servo_cote
  from module.tirette import Selection_zone
  
  # import matplotlib
  # matplotlib.use('Agg')
  # import matplotlib.pyplot as plt
  # from external.affichage_ecran import Ecran
  
  
  from robot_package.RobotControl import RobotControl
  from task_manager import Task
  import time
  
  import numpy as np
  
  #########################################
  # Creation de la stratégie
  #########################################
  
  # Déplacement
  def go_to(goal: Pose2D):
      global robotcontrol
      robotcontrol.set_goal(goal)
  
      robotcontrol.robot.enable_motors()
  
      while not(robotcontrol.goal_reached()):
          robotcontrol.update_speed()
          print(robotcontrol.robotPose)
      
      robotcontrol.robot.set_speed(0, 0)
      robotcontrol.robot.disable_motors()
  
  # Déplacement angulaire
  def go_to_angle(angle: float):
      global robotcontrol
      robotcontrol.robot.enable_motors()
  
  
      def mod(a, n):
          return (a%n+n)%n
  
      Kalpha = 0.05
  
      tmax = time.time() + 10.0
  
      Beta = 10
      while (abs(Beta) >= 0.05 ) and time.time() < tmax:
          pose = robotcontrol.robot.get_pose()
          theta = pose.theta
  
          # calcul angle Beta
          Beta = angle - theta
          Beta = mod(Beta + np.pi, 2*np.pi) - np.pi
          # print(Beta)
          ang_speed = -Kalpha * Beta
          print(robotcontrol.robot.get_pose())
  
          robotcontrol.robot.set_speed(0.0, ang_speed)
  
      robotcontrol.robot.set_speed(0, 0)
      robotcontrol.robot.disable_motors()
      
  
  def stop_robot():
      global robotcontrol
      robotcontrol.robot.set_speed(0, 0)
      robotcontrol.robot.disable_motors()
  
  
  # Attente
  def wait(t):
      time.sleep(t)
  
  
  # Modules
  def servo(valeur):
      """ Max = 1 | Mid = 0 | Min = -1 """
      global servo_cote
      servo_cote.move_mirror(valeur)
      time.sleep(0.5)
  
  
  def drapeau(valeur):
      """ levée = 1 \ -0.5 """
      global servo_drapeau
      print("lever drapeau", valeur)
      servo_drapeau.value = valeur
  
  def vitesse(v_lin: float, v_ang:float, temps:float):
      time.sleep(0.5)
      robotcontrol.robot.enable_motors()
      robotcontrol.robotPose = robotcontrol.robot.get_pose() # update controller pos
  
      t0 = time.time()
      while time.time() < t0 + temps:
          robotcontrol.robot.set_speed(v_lin, v_ang)
          print(robotcontrol.robot.get_pose())
  
      robotcontrol.robot.disable_motors()
  
  def stop_and_wait(t):
      stop_robot()
      time.sleep(t)
  
  
  
  point_depart = Pose2D(0, 0, 0)
  point_depart = Pose2D(250, 1200, np.pi/2)
  ###
  # Creation des tasks
  #################################################################################
  # task_go_point_phare = Task(lambda: go_to(Pose2D(250, 1600)), "task_go_point_phare")
  # task_stop1 = Task(lambda: stop_and_wait(2))
  task_vitesse_phare = Task(lambda: vitesse(15, 0.0, 3), "task_vitesse_phare")
  task_vitesseNeg_phare = Task(lambda: vitesse(-12, 0.0, 0.8), "task_vitesseNeg_phare")
  task_orienter_phare = Task(lambda: go_to_angle(0.0), "task_orienter_phare")
  
  task_orienter_point_inter = Task(lambda: go_to_angle(-np.pi/2), "task_orienter_point_inter")
  # task_point_MAH = Task(lambda: go_to(Pose2D(170, 170)), "task_point_MAH")
  
  # task_orienter_MAH = Task(lambda: go_to_angle(0), "task_orienter_MAH")
  # # Bras sur les côtés
  
  # task_deploye_servo = Task(lambda : servo(1), "task_deploye_servo")
  # task_point_fin_MAH = Task(lambda: go_to(Pose2D(750, 170)), "task_point_fin_MAH")
  # task_retract_servo = Task(lambda : servo(-1), "task_retract_servo")
  
  
  # Drapeau
  task_lever_drapeau = Task(lambda : drapeau(1), "task_lever_drapeau")
  task_baisser_drapeau = Task(lambda : drapeau(-0.5), "task_baisser_drapeau")
  
  # task_affichage_ecran = Task(display_ecran)
  ############################################
  # Création du robot
  ############################################
  
  # Carte moteur
  nom_fichier = './config/robot_config.yaml'
  robot_port = "/dev/ttyACM0"
  robot_baurate = 115200
  
  # Carte obstacle
  carte_obstacle_port = "/dev/ttyUSB0"
  carte_obstacle_bauderate = 115200
  
  robotcontrol = RobotControl(nom_fichier, robot_port, robot_baurate, carte_obstacle_port, carte_obstacle_bauderate)
  robotcontrol.robot.reset_board()
  for _ in range (10):
      robotcontrol.robot.set_pose(point_depart.x, point_depart.y, point_depart.theta)
  robotcontrol.robot.set_pid_left( 0.15, 0.024,  0.006)
  robotcontrol.robot.set_pid_right(0.15, 0.024,  0.006)
  print(robotcontrol.robot.get_pose())
  
  time.sleep(0.5)
  
  
  ###########################################
  # Creation des modules
  ###########################################
  # Pour les différents servos
  Device.pin_factory = RPiGPIOFactory()
  
  # Module servo sur le coté
  servo_cote = Servo_cote(27, 17)
  servo_cote.move_mirror(-1)
  
  
  servo_drapeau = Servo(22)
  servo_drapeau.value = -0.5
  
  
  #########################################
  # Main programme
  ##########################################
  selection_zone = Selection_zone(14, 15)
  selection_zone.wait_start_loop() # TIRETTE
  
  
  t0 = time.time()
  Tmax = 100 #secondes
  T_Drapeau = Tmax-5 #secondes
  # start
  t = 0
  # ecran = Ecran()
  
  tmp_task = Task(lambda: go_to(Pose2D(1000,0)))
  tmp_task = Task(lambda: go_to(Pose2D(1000,0)))
  # score = 0
  # Main loop
  while t < Tmax:
  
      # tmp_task.start()
  
      task_vitesse_phare.start()
  
      if task_vitesse_phare.done:
          task_vitesseNeg_phare.start()
      if task_vitesseNeg_phare.done:
          task_orienter_point_inter.start()
  
      # if task_orienter_point_inter.done:
      #     task_point_MAH.start()
  
      # if task_point_MAH.done:
      #     task_orienter_MAH.start()
  
      # if task_orienter_MAH.done:
  
      # if task_deploye_servo.done:
      #     task_point_fin_MAH.start()
  
      # if task_point_fin_MAH.done:
      #     task_retract_servo.start()
  
  
  
  #############################################
      if t > T_Drapeau:
          task_lever_drapeau.start()
  
  
      t = time.time() - t0
  
  task_vitesse_phare.stop()
  task_vitesseNeg_phare.stop()
  task_orienter_point_inter.stop()
  # task_point_MAH
  
  for _ in range(150):
      stop_robot() 
  
  print("FIN Programme")
  time.sleep(10)
  
  
  