QuattroRobotLab/Help_Functions.py at main · solsaem/QuattroRobotLab · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
from time import sleep
import time
try:
    from picamera2 import Picamera2, Preview
except:
    ""
import cv2
import numpy as np
import math
#################################################################################
                    ### GLOBAL CONST VARIABLES ###                              #
SPEED_LT = 61                           # LEFT MOTOR                            #
SPEED_RT = 63.75                        # RIGHT MOTOR                           #
METER_TIME = 2.3                        # TIME TO DRIVE 1 METER                 #
QUARTER_ROTATION_TIME = 0.725           # TIME TO DO QUARTER ROTATION           #
QUARTER_MOVING_ROTATION_TIME = 1.45     # TIME TO DO QUARTER MOVING ROTATION    #
CAMERA_MATRIX = np.array([[2047, 0, 1296], [0, 2047, 972], [0, 0, 1]])          #
IS_DRIVING = False                                                              #
IS_TURNING_LEFT = False                                                         #
IS_TURNING_RIGHT = False                                                        #
#################################################################################

### Takes the robot object and the degrees it should turn (to the right) ###
def TurnXDegLeft(rob, deg):
    if deg < 0:
        TurnXDegRight(rob, abs(deg))
        return
    IS_TURNING_LEFT = True
    print(rob.go_diff(SPEED_LT/2, SPEED_RT/2, 0, 1))
    sleep(deg*0.0211+0.0646)
    Stop(rob)
    IS_TURNING_LEFT = False

### Takes the robot object and the degrees it should turn (to the left) ###
def TurnXDegRight(rob, deg):
    if deg < 0:
        TurnXDegLeft(rob, abs(deg))
        return
    IS_TURNING_RIGHT = True
    print(rob.go_diff(SPEED_LT/2, SPEED_RT/2, 1, 0))
    sleep(deg*0.0211+0.0646)
    Stop(rob)
    IS_TURNING_RIGHT = False

### Takes the robot object, the distance, and direction (negative for backwards) ###
def GoXCM(rob, dist, dir, speed):
    ret = 1
    start = time.perf_counter()
    while time.perf_counter() - start < dist*0.0264 - 0.0608:
        left = rob.read_left_ping_sensor()
        right = rob.read_right_ping_sensor()
        front = rob.read_front_ping_sensor()
        if right < 250 or left < 250 or front < 250:
            print("Arlo too close")
            print(left)
            print(right)
            print(front)
            ret = 0
            break
        IS_DRIVING = True
        print(rob.go_diff(SPEED_LT * speed, SPEED_RT * speed, dir, dir))
    Stop(rob)
    IS_DRIVING = False
    return ret
### Stops the robot ###
def Stop(rob):
    print(rob.stop())
    sleep(0.1)

### Takes the robot object, degrees it should turn, and the direction (positve = right, negative = left) ###
def MovingTurn(rob, deg, dir): # dir = 1 (right turn); dir = -1 (left turn)
    print(rob.go_diff(SPEED_LT + (30 * dir), SPEED_RT - (30 * dir), 1, 1))
    sleep((deg*QUARTER_MOVING_ROTATION_TIME)/90)

### Takes the robot position, robot radius, an object position, and object radius. Returns true if they don't collide)
def Collision(robot_point, robot_radius, object_point, object_radius):
    return np.sqrt((robot_point[0] - object_point[0]) ** 2 + (robot_point[1] - object_point[1]) ** 2) < (robot_radius + object_radius)

### Start the camera with high resolution and return the picam ###
def Camera_Init():
    picam2 = Picamera2()
    camera_config = picam2.create_preview_configuration({"size": (2592, 1944)})
    picam2.configure(camera_config)
    picam2.start()
    time.sleep(2)
    return picam2


# Return True on collision
def check_collision(p1, p2, r1, r2):
    return np.linalg.norm(np.array(p1) - np.array(p2)) <= r1 + r2

# Calculates distance between two points
def calculate_distance(p1, p2):
    return np.linalg.norm(np.array(p1) - np.array(p2))

def calculate_angle(x, y, x_prev, y_prev):
    delta_x = x - x_prev
    delta_y = y - y_prev

    angle = math.atan2(delta_x, delta_y)

    angle_degrees = math.degrees(angle)

    return angle_degrees