API tools faq
paste
Advertisement
Guest User

Untitled

a guest
May 25th, 2018
83
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 11.83 KB | None | 0 0
raw download clone embed print report
  1. #!/usr/bin/env python3
  2. from datetime import time
  3. import random
  4.  
  5. from ev3dev.ev3 import *
  6. import time
  7.  
  8.  
  9. def current_time():
  10.     return int(round(time.time() * 1000))
  11.  
  12.  
  13. def main():
  14.     robot = Robot("outB", "outC")
  15.  
  16.     while True:
  17.         directions = robot.process_colors(robot.register_colors(robot.forward_color()))
  18.         print(directions)
  19.  
  20.         if len(directions) == 0:
  21.             continue
  22.  
  23.         if Robot.WIN in directions:
  24.             break
  25.  
  26.         print(directions)
  27.  
  28.         if len(directions) > 1 and Robot.D_BACKWARD in directions:
  29.             directions.remove(Robot.D_BACKWARD)
  30.  
  31.         print(directions)
  32.         print("----")
  33.  
  34.         robot.rotate_to_direction(random.sample(directions, 1)[0])
  35.         robot.forward(duration=500)
  36.  
  37.  
  38. class Robot:
  39.     DEFAULT_SPEED_FORWARD = 90
  40.     DEFAULT_SPEED_BACKWARD = 45
  41.     DEFAULT_ROTATE_SPEED = 90
  42.     DEFAULT_TURN_SPEED = 90
  43.     C_YELLOW = "GREEN_YELLOW"
  44.     C_BLUE = "BLUE"
  45.     C_RED = "RED"
  46.     C_BLACK = "BLACK"
  47.     D_LEFT = "L"
  48.     D_FORWARD = "F"
  49.     D_BACKWARD = "B"
  50.     D_RIGHT = "R"
  51.     WIN = "WIN"
  52.  
  53.     def __init__(self, left_motor, right_motor):
  54.         """
  55.        Initialise le robot, ses moteurs et capteurs.
  56.        :param left_motor: identifiant du moteur de gauche
  57.        :param right_motor: identifiant du moteur de droite
  58.        """
  59.         self.leftMotor = LargeMotor(left_motor)
  60.         self.leftMotor.stop_action = LargeMotor.STOP_ACTION_HOLD
  61.         self.rightMotor = LargeMotor(right_motor)
  62.         self.rightMotor.stop_action = LargeMotor.STOP_ACTION_HOLD
  63.         self.gyroscope = GyroSensor()
  64.         self.colorSensor = ColorSensor()
  65.         self.colorSensor.mode = ColorSensor.MODE_RGB_RAW
  66.         self.gyroscope.mode = 'GYRO-RATE'
  67.         self.gyroscope.mode = 'GYRO-ANG'
  68.  
  69.     def stop(self):
  70.         """
  71.        Arrete les moteurs du robot.
  72.        """
  73.         self.leftMotor.stop()
  74.         self.rightMotor.stop()
  75.  
  76.     def forward(self, speed=DEFAULT_SPEED_FORWARD, duration=0):
  77.         """
  78.        Fait avancer le robot avec une vitesse et une duree parametrable.
  79.        :param speed: vitesse du robot; defaut: DEFAULT_SPEED_WORWARD
  80.        :param duration: duree du trajet; 0 pour infini; defaut: 0
  81.        """
  82.         self.leftMotor.run_forever(speed_sp=speed)
  83.         self.rightMotor.run_forever(speed_sp=speed)
  84.         if duration != 0:
  85.             time.sleep(duration)
  86.             self.stop()
  87.  
  88.     def backward(self, speed=DEFAULT_SPEED_BACKWARD, duration=0):
  89.         """
  90.        Fait reculer le robot avec une vitesse et une duree parametrable.
  91.        :param speed: vitesse du robot; defaut: DEFAULT_SPEED_BACKWARD
  92.        :param duration: duree du trajet; 0 pour infini; deaut: 0
  93.        """
  94.         self.forward(-speed, duration)
  95.  
  96.     def get_color(self):
  97.         """
  98.        Retourne la couleur que capte le capteur au moment de l'appel de la fonction.
  99.        :return: couleur detectee
  100.        """
  101.         return self.colorSensor.color
  102.  
  103.     def turn(self, deg, speed=DEFAULT_TURN_SPEED):
  104.         """
  105.        Fait tourner le robot d'un certain angle puis le fait avance jusqu'a l'arret.
  106.        :param deg: angle du virage
  107.        :param speed: vitesse du virage; defaut: DEFAULT_TURN_SPEED
  108.        """
  109.         speed_left = self.leftMotor.speed
  110.         speed_right = self.rightMotor.speed
  111.         self.stop()
  112.         if deg > 0:
  113.             self.leftMotor.run_forever(speed_sp=speed)
  114.         else:
  115.             self.rightMotor.run_forever(speed_sp=speed)
  116.         start_gyroscope_value = self.gyroscope.value()
  117.         while abs(self.gyroscope.value() - start_gyroscope_value) < abs(deg):
  118.             pass
  119.         self.stop()
  120.         self.leftMotor.run_forever(speed_sp=speed_left)
  121.         self.rightMotor.run_forever(speed_sp=speed_right)
  122.  
  123.     def rotate(self, deg, speed=DEFAULT_ROTATE_SPEED):
  124.         """
  125.        Fait tourner le robot d'un certain angle autour de lui meme.
  126.        :param deg: angle de la rotation
  127.        :param speed: votesse de la rotation; defaut: DEFAULT_ROTATE_SPEED
  128.        """
  129.         self.stop()
  130.         if deg > 0:
  131.             self.leftMotor.run_forever(speed_sp=speed)
  132.             self.rightMotor.run_forever(speed_sp=-speed)
  133.         else:
  134.             self.leftMotor.run_forever(speed_sp=-speed)
  135.             self.rightMotor.run_forever(speed_sp=speed)
  136.         start_gyroscope_value = self.gyroscope.value()
  137.         while abs(self.gyroscope.value() - start_gyroscope_value) < abs(deg):
  138.             pass
  139.         self.stop()
  140.  
  141.     def rotate90(self, deg, speed=DEFAULT_ROTATE_SPEED):
  142.         """
  143.        Fait faire une rotation d'exactement 90° au robot.
  144.        :param deg: positif ou negatif: direction de la rotation
  145.        :param speed: vitesse de la rotation; defaut: DEFAULT_ROTATE_SPEED
  146.        """
  147.         self.stop()
  148.         if deg > 0:
  149.             self.leftMotor.run_forever(speed_sp=speed)
  150.             self.rightMotor.run_forever(speed_sp=-speed)
  151.         else:
  152.             self.leftMotor.run_forever(speed_sp=-speed)
  153.             self.rightMotor.run_forever(speed_sp=speed)
  154.         start_gyroscope_value = self.gyroscope.value()
  155.         while abs(self.gyroscope.value() - start_gyroscope_value) < 90:
  156.             pass
  157.         self.stop()
  158.         self.right_angle()
  159.  
  160.     def rotate_to_direction(self, direction):
  161.         """
  162.        Fait tourner le robot jusqu'a ce qu'il atteigne une direction (D_WORWARD, D_LEFT, D_BACKWARD, D_RIGHT)
  163.        :param direction: direction a laquelle le robot doit faire face
  164.        """
  165.         if direction == self.D_FORWARD:
  166.             pass
  167.         if direction == self.D_LEFT:
  168.             self.rotate90(-1)
  169.         elif direction == self.D_BACKWARD:
  170.             self.rotate90(-1)
  171.             self.rotate90(-1)
  172.         elif direction == self.D_RIGHT:
  173.             self.rotate90(1)
  174.  
  175.     def forward_color(self):
  176.         """
  177.        Fait avance le robot jusqu'a ce qu'il atteigne une couleur
  178.        :return: couleur obtenue
  179.        """
  180.         self.forward()
  181.         while True:
  182.             red = self.colorSensor.value(0)
  183.             green = self.colorSensor.value(1)
  184.             blue = self.colorSensor.value(2)
  185.             if (red < 120 and green < 120 and blue < 120) or (red > 120 and green < 100 and blue < 80) or (
  186.                     red < 120 and green < 200 and blue > 200) or (red > 110 and green > 140 and blue < 70):
  187.                 print({"red": red, "green": green, "blue": blue})
  188.                 return {"red": red, "green": green, "blue": blue}
  189.  
  190.     def print_color(self):
  191.         """
  192.        Affiche la couleur que le capteur capte au moment de l'appel de la fonction
  193.        """
  194.         print("(" + str(self.colorSensor.value(0)), end='')
  195.         print(", " + str(self.colorSensor.value(1)), end='')
  196.         print(", ", str(self.colorSensor.value(2)) + ")")
  197.  
  198.     def register_colors(self, rgb):
  199.         """
  200.        Enregistre les bandes de couleurs detectees par le robot pendant 1750ms
  201.        :param rgb: premiere couleur detectee par forward_color
  202.        :return: liste des couleurs detectes
  203.        """
  204.         colors = list()
  205.         if rgb["red"] < 120 and rgb["green"] < 120 and rgb["blue"] < 120:
  206.             colors.append(self.C_BLACK)
  207.         elif rgb["red"] > 120 and rgb["green"] < 100 and rgb["blue"] < 80:
  208.             colors.append(self.C_RED)
  209.         elif rgb["red"] < 120 and rgb["green"] < 200 and rgb["blue"] > 200:
  210.             colors.append(self.C_BLUE)
  211.         elif rgb["red"] > 110 and rgb["green"] > 140 and rgb["blue"] < 70:
  212.             colors.append(self.C_YELLOW)
  213.         last = True
  214.         start_time = current_time()
  215.         while current_time() < start_time + 1750:
  216.             red = self.colorSensor.value(0)
  217.             green = self.colorSensor.value(1)
  218.             blue = self.colorSensor.value(2)
  219.             if red < 120 and green < 120 and blue < 120:
  220.                 if not last:
  221.                     last = True
  222.                     colors.append(self.C_BLACK)
  223.             elif red > 120 and green < 100 and blue < 80:
  224.                 if not last:
  225.                     last = True
  226.                     colors.append(self.C_RED)
  227.             elif red < 120 and green < 200 and blue > 200:
  228.                 if not last:
  229.                     last = True
  230.                     colors.append(self.C_BLUE)
  231.             elif red > 110 and green > 140 and blue < 70:
  232.                 if not last:
  233.                     last = True
  234.                     colors.append(self.C_YELLOW)
  235.             else:
  236.                 last = False
  237.         print(colors)
  238.         return colors
  239.  
  240.     def right_angle(self):
  241.         """
  242.        S'assure que le robot est à un angle droit
  243.        """
  244.         if (self.gyroscope.value() % 360) % 90 != 0:
  245.             if (self.gyroscope.value() % 360) < 45:
  246.                 self.rotate(0 - (self.gyroscope.value() % 360))
  247.             elif (self.gyroscope.value() % 360) < 90:
  248.                 self.rotate(90 - (self.gyroscope.value() % 360))
  249.             elif (self.gyroscope.value() % 360) < 135:
  250.                 self.rotate(90 - (self.gyroscope.value() % 360))
  251.             elif (self.gyroscope.value() % 360) < 180:
  252.                 self.rotate(180 - (self.gyroscope.value() % 360))
  253.             elif (self.gyroscope.value() % 360) < 225:
  254.                 self.rotate(180 - (self.gyroscope.value() % 360))
  255.             elif (self.gyroscope.value() % 360) < 270:
  256.                 self.rotate(270 - (self.gyroscope.value() % 360))
  257.             elif (self.gyroscope.value() % 360) < 315:
  258.                 self.rotate(270 - (self.gyroscope.value() % 360))
  259.             elif (self.gyroscope.value() % 360) < 360:
  260.                 self.rotate(360 - (self.gyroscope.value() % 360))
  261.  
  262.     def process_colors(self, colors):
  263.         """
  264.        Traite les couleurs obtenues et retourne les directions possibles.
  265.        :param colors: liste des oculeurs obtenues par register_color
  266.        :return: directions possibles
  267.        """
  268.         directions = set()
  269.         if colors.count(self.C_BLACK) > 1:
  270.             directions.add(self.WIN)
  271.         else:
  272.             temp = self.gyroscope.value() % 360
  273.             if self.C_BLACK in colors:
  274.                 if 45 <= temp < 135:
  275.                     directions.add(self.D_LEFT)
  276.                 elif 135 <= temp < 225:
  277.                     directions.add(self.D_BACKWARD)
  278.                 elif 225 <= temp < 315:
  279.                     directions.add(self.D_RIGHT)
  280.                 else:
  281.                     directions.add(self.D_FORWARD)
  282.             if self.C_RED in colors:
  283.                 if 45 <= temp < 135:
  284.                     directions.add(self.D_BACKWARD)
  285.                 elif 135 <= temp < 225:
  286.                     directions.add(self.D_RIGHT)
  287.                 elif 225 <= temp < 315:
  288.                     directions.add(self.D_FORWARD)
  289.                 else:
  290.                     directions.add(self.D_LEFT)
  291.             if self.C_BLUE in colors:
  292.                 if 45 <= temp < 135:
  293.                     directions.add(self.D_FORWARD)
  294.                 elif 135 <= temp < 225:
  295.                     directions.add(self.D_LEFT)
  296.                 elif 225 <= temp < 315:
  297.                     directions.add(self.D_BACKWARD)
  298.                 else:
  299.                     directions.add(self.D_RIGHT)
  300.             if self.C_YELLOW in colors:
  301.                 if 45 <= temp < 135:
  302.                     directions.add(self.D_RIGHT)
  303.                 elif 135 <= temp < 225:
  304.                     directions.add(self.D_FORWARD)
  305.                 elif 225 <= temp < 315:
  306.                     directions.add(self.D_LEFT)
  307.                 else:
  308.                     directions.add(self.D_BACKWARD)
  309.         return directions
  310.  
  311.  
  312. try:
  313.     # point d'entree du programme
  314.     main()
  315. finally:
  316.     # force le robot a s'arreter si le programme a une erreur ou est brutalement arrete
  317.     left = LargeMotor("outB")
  318.     right = LargeMotor("outC")
  319.     left.stop()
  320.     right.stop()
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!