Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- """Objektid."""
- from PiBot import PiBot
- import math
- import time
- robot = PiBot()
- speed = 8
- sleep = 0.05
- robot.set_grabber_height(100)
- buffer = 0.1
- def smooth_straight(angular_speed):
- """Robot moves straight and tries to maintain given angular speed"""
- start_time = time.time()
- start_e_right = robot.get_right_wheel_encoder()
- start_e_left = robot.get_left_wheel_encoder()
- time_from_start = 0
- power = 13
- increment = 1
- robot.set_wheels_speed(power)
- while time_from_start < 10:
- delta_time = time.time() - start_time - time_from_start
- time_from_start = time.time() - start_time
- degrees_from_start_r = robot.get_right_wheel_encoder() - start_e_right
- degrees_from_start_l = robot.get_left_wheel_encoder() - start_e_left
- speed_right = degrees_from_start_r / time_from_start
- speed_left = degrees_from_start_l / time_from_start
- if speed_right > angular_speed:
- robot.set_right_wheel_speed(power - increment)
- if speed_right < angular_speed:
- robot.set_right_wheel_speed(power + increment)
- if speed_left > angular_speed:
- robot.set_left_wheel_speed(power - increment)
- if speed_left < angular_speed:
- robot.set_left_wheel_speed(power + increment)
- # robot.sleep(sleep)
- print(f"time_from_start = {time_from_start}")
- print(f"delta_time = {delta_time}")
- print(f"speed_right = {speed_right}")
- print(f"speed_left = {speed_left}")
- print(f"degrees_from_start_r = {degrees_from_start_r}")
- print(f"degrees_from_start_l = {degrees_from_start_l}")
- print("xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx")
- def smooth_left(angular_speed):
- """Robot turns left and tries to maintain given angular speed"""
- time_from_start = 0
- power = 13
- increment = 5
- start_time = time.time()
- start_rotation = robot.get_rotation()
- robot.set_right_wheel_speed(power)
- robot.set_left_wheel_speed(-power)
- while time_from_start < 10:
- delta_time = time.time() - start_time - time_from_start
- time_from_start = time.time() - start_time
- rotation_from_start = robot.get_rotation() - start_rotation
- rotation_speed = rotation_from_start / time_from_start
- if rotation_speed > angular_speed:
- robot.set_right_wheel_speed(power - increment)
- robot.set_left_wheel_speed(-power + increment)
- if rotation_speed < angular_speed:
- robot.set_right_wheel_speed(power + increment)
- robot.set_left_wheel_speed(-power - increment)
- # robot.sleep(sleep)
- print(f"time_from_start = {time_from_start}")
- print(f"delta_time = {delta_time}")
- print(f"rotation_speed = {rotation_speed}")
- print("xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx")
- def smooth_left2(angular_speed):
- """Robot moves straight and tries to maintain given angular speed"""
- start_time = time.time()
- start_e_right = robot.get_right_wheel_encoder()
- start_e_left = robot.get_left_wheel_encoder()
- time_from_start = 0
- delta_time = 0
- power = 13
- robot.set_right_wheel_speed(power)
- robot.set_left_wheel_speed(-power)
- while time_from_start < 10:
- time_from_start = time.time() - start_time
- degrees_from_start_r = robot.get_right_wheel_encoder() - start_e_right
- degrees_from_start_l = robot.get_left_wheel_encoder() - start_e_left
- speed_right = degrees_from_start_r / time_from_start
- speed_left = degrees_from_start_l / time_from_start
- if speed_right > angular_speed:
- robot.set_right_wheel_speed(power - 2)
- if speed_right < angular_speed:
- robot.set_right_wheel_speed(power + 2)
- if speed_left > angular_speed:
- robot.set_left_wheel_speed(-power + 2)
- if speed_left < angular_speed:
- robot.set_left_wheel_speed(-power - 2)
- robot.sleep(sleep)
- print(f"time_from_start = {time_from_start}")
- print(f"delta_time = {delta_time}")
- print(f"speed_right = {speed_right}")
- print(f"speed_left = {speed_left}")
- print(f"degrees_from_start_r = {degrees_from_start_r}")
- print(f"degrees_from_start_l = {degrees_from_start_l}")
- print("xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx")
- def anti_left():
- """Robot spins/ turns backwards left."""
- global speed
- robot.set_left_wheel_speed(speed)
- robot.set_right_wheel_speed(-speed)
- robot.sleep(sleep)
- def left():
- """Robot spins/ turns left."""
- global speed
- robot.set_left_wheel_speed(-speed)
- robot.set_right_wheel_speed(speed)
- robot.sleep(sleep)
- def straight():
- """Robot moves straight."""
- robot.set_wheels_speed(speed)
- robot.sleep(sleep)
- def solve_triangle(a, b, angle):
- """Find side and angle of current triangle."""
- side = math.sqrt(a ** 2 + b ** 2 + 2 * a * b * math.cos(angle))
- return side
- def position_finder(master_list):
- """Scan the list and find the objects."""
- print("alustan position finderit")
- global buffer
- two_objects_list = []
- for i in master_list:
- if len(two_objects_list) == 2:
- two_objects_list.sort(key=lambda x: x[1])
- return two_objects_list
- elif i[0] > buffer:
- two_objects_list.append(i)
- def make_master_list():
- """Spin around and make a list of, what u see."""
- global buffer
- b = robot.get_front_middle_laser()
- a = robot.get_front_middle_laser()
- list_of_everything = []
- while abs(robot.get_rotation()) < 360:
- xx = 360 - robot.get_rotation()
- print(f"Delta middle laser = {abs(a - b)}")
- print(f"Delta encoder = {xx}")
- print("xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx")
- left()
- list_of_everything.append([abs(a - b), xx, b])
- a = b
- b = robot.get_front_middle_laser()
- print("ring sai täis")
- print(list_of_everything)
- robot.set_wheels_speed(0)
- return list_of_everything
- def turn_back(two_object_list):
- """Turn back to the rotation, where the object is."""
- print("alustasin turn backi")
- place = two_object_list[0][1]
- while (360 - (robot.get_rotation())) < place:
- anti_left()
- print(f" rotation = {360 - robot.get_rotation()}")
- robot.set_wheels_speed(0)
- print("Asukoht lukus")
- def encoder_finder(distance):
- """How many degrees needed to spin."""
- robot.close_grabber(0)
- a = robot.get_left_wheel_encoder()
- b = robot.WHEEL_DIAMETER
- c = 3.14 * b
- d = distance / c
- e = d * 360
- while robot.get_left_wheel_encoder() < a + e:
- straight()
- robot.set_wheels_speed(0)
- def move_to_object(two_object_list):
- """Robot goes to object and corrects itself."""
- print("alustan move to object")
- distance = two_object_list[0][2]
- robot.close_grabber(0)
- encoder_finder(distance)
- # while robot.get_front_middle_laser() > 0.1:
- # straight()
- # print(robot.get_front_middle_laser())
- robot.set_wheels_speed(0)
- print("kohal!!!")
- def turn_to_correct_angle(two_object_list):
- """When next to the object turn to the correct angle."""
- angle = two_object_list[0][1] - two_object_list[1][1]
- needed_angle = 60 - angle
- start_rotation = robot.get_rotation()
- while start_rotation + needed_angle < robot.get_rotation():
- anti_left()
- def back_to_correct_place(two_object_list):
- """Back out to the correct location until laser sees same distance as length between two objects."""
- first_length = two_object_list[0][2]
- second_length = two_object_list[1][2]
- angle = two_object_list[0][1] - two_object_list[1][1]
- correct_length = solve_triangle(first_length, second_length, angle)
- encoder_finder(correct_length)
- list_of_everything = make_master_list()
- list_of_objects = position_finder(list_of_everything)
- print(f"first object positions = {list_of_objects[0]}")
- print(f"first object positions = {list_of_objects[1]}")
- turn_back(list_of_objects)
- move_to_object(list_of_objects)
- turn_to_correct_angle(list_of_objects)
- back_to_correct_place(list_of_objects)
- # smooth_straight(180)
- # smooth_left(130)
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement