API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Apr 19th, 2018
64
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 9.33 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. # robot.print_color()
  18. # time.sleep(0.1)
  19.  
  20. while True:
  21. directions = robot.process_colors(robot.register_colors(robot.forward_color()))
  22. print(directions)
  23.  
  24. if len(directions) == 0:
  25. continue
  26.  
  27. if Robot.WIN in directions:
  28. break
  29.  
  30. print(directions)
  31.  
  32. if len(directions) > 1 and Robot.D_BACKWARD in directions:
  33. directions.remove(Robot.D_BACKWARD)
  34.  
  35. print(directions)
  36. print("----")
  37.  
  38. robot.rotate_to_direction(random.sample(directions, 1)[0])
  39. robot.forward(duration=500)
  40.  
  41.  
  42. class Robot:
  43. DEFAULT_SPEED_FORWARD = 90
  44. DEFAULT_SPEED_BACKWARD = 45
  45. DEFAULT_ROTATE_SPEED = 90
  46. DEFAULT_TURN_SPEED = 90
  47. C_YELLOW = "GREEN_YELLOW"
  48. C_BLUE = "BLUE"
  49. C_RED = "RED"
  50. C_BLACK = "BLACK"
  51. D_LEFT = "L"
  52. D_FORWARD = "F"
  53. D_BACKWARD = "B"
  54. D_RIGHT = "R"
  55. WIN = "WIN"
  56.  
  57. def __init__(self, left_motor, right_motor):
  58. self.leftMotor = LargeMotor(left_motor)
  59. self.leftMotor.stop_action = LargeMotor.STOP_ACTION_HOLD
  60. self.rightMotor = LargeMotor(right_motor)
  61. self.rightMotor.stop_action = LargeMotor.STOP_ACTION_HOLD
  62. self.gyroscope = GyroSensor()
  63. self.colorSensor = ColorSensor()
  64. self.colorSensor.mode = ColorSensor.MODE_RGB_RAW
  65. self.gyroscope.mode = 'GYRO-RATE'
  66. self.gyroscope.mode = 'GYRO-ANG'
  67.  
  68. def stop(self):
  69. self.leftMotor.stop()
  70. self.rightMotor.stop()
  71.  
  72. def forward(self, speed=DEFAULT_SPEED_FORWARD, duration=0):
  73. self.leftMotor.run_forever(speed_sp=speed)
  74. self.rightMotor.run_forever(speed_sp=speed)
  75. if duration != 0:
  76. time.sleep(duration)
  77. self.stop()
  78.  
  79. def backward(self, speed=DEFAULT_SPEED_BACKWARD, duration=0):
  80. self.forward(-speed, duration)
  81.  
  82. def get_color(self):
  83. return self.colorSensor.color
  84.  
  85. def turn(self, deg, speed=DEFAULT_TURN_SPEED):
  86. speed_left = self.leftMotor.speed
  87. speed_right = self.rightMotor.speed
  88. self.stop()
  89. if deg > 0:
  90. self.leftMotor.run_forever(speed_sp=speed)
  91. else:
  92. self.rightMotor.run_forever(speed_sp=speed)
  93. start_gyroscope_value = self.gyroscope.value()
  94. while abs(self.gyroscope.value() - start_gyroscope_value) < abs(deg):
  95. pass
  96. self.stop()
  97. self.leftMotor.run_forever(speed_sp=speed_left)
  98. self.rightMotor.run_forever(speed_sp=speed_right)
  99.  
  100. def rotate(self, deg, speed=DEFAULT_ROTATE_SPEED):
  101. self.stop()
  102. if deg > 0:
  103. self.leftMotor.run_forever(speed_sp=speed)
  104. self.rightMotor.run_forever(speed_sp=-speed)
  105. else:
  106. self.leftMotor.run_forever(speed_sp=-speed)
  107. self.rightMotor.run_forever(speed_sp=speed)
  108. start_gyroscope_value = self.gyroscope.value()
  109. while abs(self.gyroscope.value() - start_gyroscope_value) < abs(deg):
  110. pass
  111. self.stop()
  112.  
  113. def rotate90(self, deg, speed=DEFAULT_ROTATE_SPEED):
  114. self.stop()
  115. if deg > 0:
  116. self.leftMotor.run_forever(speed_sp=speed)
  117. self.rightMotor.run_forever(speed_sp=-speed)
  118. else:
  119. self.leftMotor.run_forever(speed_sp=-speed)
  120. self.rightMotor.run_forever(speed_sp=speed)
  121. start_gyroscope_value = self.gyroscope.value()
  122. while abs(self.gyroscope.value() - start_gyroscope_value) < 90:
  123. pass
  124. self.stop()
  125. self.right_angle()
  126.  
  127. def rotate_to_direction(self, direction):
  128. if direction == self.D_FORWARD:
  129. pass
  130. if direction == self.D_LEFT:
  131. self.rotate90(-1)
  132. elif direction == self.D_BACKWARD:
  133. self.rotate90(-1)
  134. self.rotate90(-1)
  135. elif direction == self.D_RIGHT:
  136. self.rotate90(1)
  137.  
  138. def forward_color(self):
  139. self.forward()
  140. while True:
  141. red = self.colorSensor.value(0)
  142. green = self.colorSensor.value(1)
  143. blue = self.colorSensor.value(2)
  144. if (red < 120 and green < 120 and blue < 120) or (red > 120 and green < 100 and blue < 80) or (
  145. red < 120 and green < 200 and blue > 200) or (red > 110 and green > 140 and blue < 70):
  146. print({"red": red, "green": green, "blue": blue})
  147. return {"red": red, "green": green, "blue": blue}
  148.  
  149. def print_color(self):
  150. print("(" + str(self.colorSensor.value(0)), end='')
  151. print(", " + str(self.colorSensor.value(1)), end='')
  152. print(", ", str(self.colorSensor.value(2)) + ")")
  153.  
  154. def register_colors(self, rgb):
  155. colors = list()
  156. if rgb["red"] < 120 and rgb["green"] < 120 and rgb["blue"] < 120:
  157. colors.append(self.C_BLACK)
  158. elif rgb["red"] > 120 and rgb["green"] < 100 and rgb["blue"] < 80:
  159. colors.append(self.C_RED)
  160. elif rgb["red"] < 120 and rgb["green"] < 200 and rgb["blue"] > 200:
  161. colors.append(self.C_BLUE)
  162. elif rgb["red"] > 110 and rgb["green"] > 140 and rgb["blue"] < 70:
  163. colors.append(self.C_YELLOW)
  164. last = True
  165. start_time = current_time()
  166. while current_time() < start_time + 1750:
  167. red = self.colorSensor.value(0)
  168. green = self.colorSensor.value(1)
  169. blue = self.colorSensor.value(2)
  170. if red < 120 and green < 120 and blue < 120:
  171. if not last:
  172. last = True
  173. colors.append(self.C_BLACK)
  174. elif red > 120 and green < 100 and blue < 80:
  175. if not last:
  176. last = True
  177. colors.append(self.C_RED)
  178. elif red < 120 and green < 200 and blue > 200:
  179. if not last:
  180. last = True
  181. colors.append(self.C_BLUE)
  182. elif red > 110 and green > 140 and blue < 70:
  183. if not last:
  184. last = True
  185. colors.append(self.C_YELLOW)
  186. else:
  187. last = False
  188. print(colors)
  189. return colors
  190.  
  191. def right_angle(self):
  192. if (self.gyroscope.value() % 360) % 90 != 0:
  193. if (self.gyroscope.value() % 360) < 45:
  194. self.rotate(0 - (self.gyroscope.value() % 360))
  195. elif (self.gyroscope.value() % 360) < 90:
  196. self.rotate(90 - (self.gyroscope.value() % 360))
  197. elif (self.gyroscope.value() % 360) < 135:
  198. self.rotate(90 - (self.gyroscope.value() % 360))
  199. elif (self.gyroscope.value() % 360) < 180:
  200. self.rotate(180 - (self.gyroscope.value() % 360))
  201. elif (self.gyroscope.value() % 360) < 225:
  202. self.rotate(180 - (self.gyroscope.value() % 360))
  203. elif (self.gyroscope.value() % 360) < 270:
  204. self.rotate(270 - (self.gyroscope.value() % 360))
  205. elif (self.gyroscope.value() % 360) < 315:
  206. self.rotate(270 - (self.gyroscope.value() % 360))
  207. elif (self.gyroscope.value() % 360) < 360:
  208. self.rotate(360 - (self.gyroscope.value() % 360))
  209.  
  210. def process_colors(self, colors):
  211. directions = set()
  212. if colors.count(self.C_BLACK) > 1:
  213. directions.add(self.WIN)
  214. else:
  215. temp = self.gyroscope.value() % 360
  216. if self.C_BLACK in colors:
  217. if 45 <= temp < 135:
  218. directions.add(self.D_LEFT)
  219. elif 135 <= temp < 225:
  220. directions.add(self.D_BACKWARD)
  221. elif 225 <= temp < 315:
  222. directions.add(self.D_RIGHT)
  223. else:
  224. directions.add(self.D_FORWARD)
  225. if self.C_RED in colors:
  226. if 45 <= temp < 135:
  227. directions.add(self.D_BACKWARD)
  228. elif 135 <= temp < 225:
  229. directions.add(self.D_RIGHT)
  230. elif 225 <= temp < 315:
  231. directions.add(self.D_FORWARD)
  232. else:
  233. directions.add(self.D_LEFT)
  234. if self.C_BLUE in colors:
  235. if 45 <= temp < 135:
  236. directions.add(self.D_FORWARD)
  237. elif 135 <= temp < 225:
  238. directions.add(self.D_LEFT)
  239. elif 225 <= temp < 315:
  240. directions.add(self.D_BACKWARD)
  241. else:
  242. directions.add(self.D_RIGHT)
  243. if self.C_YELLOW in colors:
  244. if 45 <= temp < 135:
  245. directions.add(self.D_RIGHT)
  246. elif 135 <= temp < 225:
  247. directions.add(self.D_FORWARD)
  248. elif 225 <= temp < 315:
  249. directions.add(self.D_LEFT)
  250. else:
  251. directions.add(self.D_BACKWARD)
  252. return directions
  253.  
  254.  
  255. try:
  256. main()
  257. finally:
  258. left = LargeMotor("outB")
  259. right = LargeMotor("outC")
  260. left.stop()
  261. 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!