Untitled

a guest
Oct 21st, 2019
84
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
1. """Joonejärgija."""
2. from PiBot import PiBot
3. from math import pi
4.
5. robot = PiBot()
6.
7. speed = 13
8. sleep = 0.05
9. robot.set_grabber_height(100)
10. wheel_d = robot.WHEEL_DIAMETER
11. robot_d = robot.AXIS_LENGTH
12. puhver = 0.1
13.
14. """
15. get_front_left_laser()
16. get_front_middle_laser()
17. get_front_right_laser()
18. get_front_lasers()
19. """
20.
21.
22. def left():
23. print("left")
24. global speed
25. """Robot spins/ turns left."""
26. robot.set_left_wheel_speed(0)
27. robot.set_right_wheel_speed(speed)
28. robot.sleep(sleep)
29.
30.
31. def right():
32. print("right")
33. global speed
34. """Robot spins/ turns right."""
35. robot.set_right_wheel_speed(0)
36. robot.set_left_wheel_speed(speed)
37. robot.sleep(sleep)
38.
39.
40. def straight():
41. print("straight")
42. """Robot moves straight."""
43. robot.set_wheels_speed(speed)
44.
45.
46. def find_encoder(wheel_d, robot_d):
47. return 360 * ((pi * wheel_d) / (2 * pi * robot_d))
48.
49.
50. def moves_to_object():
51. """Robot goes to object and corrects itself."""
52. print("moving")
53. straight()
54.
55.
56. def find_object():
57. global puhver
58. encoder = find_encoder(wheel_d, robot_d)
59. b = robot.get_front_middle_laser()
60. a = robot.get_front_middle_laser()
61. while encoder > robot.get_left_wheel_encoder():
62. print("finding object")
63. print(abs(a - b))
64. left()
65. a = b
66. b = robot.get_front_middle_laser()
67. if abs(a - b) > puhver:
68. if robot.get_front_middle_laser() < 0.15:
69. robot.set_wheels_speed(0)
70. break
71. else:
72. moves_to_object()
73.
74.
75. while True:
76. print("start")
77. find_object()
RAW Paste Data