210 lines
6.6 KiB
Python
210 lines
6.6 KiB
Python
from time import sleep
|
|
|
|
import cv2
|
|
import numpy as np
|
|
|
|
from .robot import Robot
|
|
|
|
START_VALUES = [60, 60]
|
|
THRESHOLD = 1.05
|
|
TESTING_SLEEP_TIME = 2
|
|
DEFAULT_CALIBRATION_CODE = "40.40-40.40-40.40_ff-ff-ff-ff"
|
|
|
|
FOCAL_LENGTH = 1691
|
|
|
|
CAMERA_MATRIX = np.array(
|
|
[[FOCAL_LENGTH,0,512],[0,FOCAL_LENGTH,360],[0,0,1]],
|
|
dtype=np.float32
|
|
)
|
|
DIST_COEF = np.array([0,0,0,0,0], dtype=np.float32)
|
|
|
|
def test_forward(arlo, l_power, r_power):
|
|
arlo.reset_encoder_counts()
|
|
arlo.go_diff(l_power, r_power, 1, 1)
|
|
sleep(TESTING_SLEEP_TIME)
|
|
arlo.stop()
|
|
return (
|
|
abs(int(arlo.read_left_wheel_encoder())),
|
|
abs(int(arlo.read_right_wheel_encoder()))
|
|
)
|
|
|
|
def test_back(arlo, l_power, r_power):
|
|
arlo.reset_encoder_counts()
|
|
arlo.go_diff(l_power, r_power, 0, 0)
|
|
sleep(TESTING_SLEEP_TIME)
|
|
arlo.stop()
|
|
return (
|
|
abs(int(arlo.read_left_wheel_encoder())),
|
|
abs(int(arlo.read_right_wheel_encoder()))
|
|
)
|
|
|
|
def test_clockwise(arlo, l_power, r_power):
|
|
arlo.reset_encoder_counts()
|
|
arlo.go_diff(l_power, r_power, 1, 0)
|
|
sleep(TESTING_SLEEP_TIME)
|
|
arlo.stop()
|
|
return (
|
|
abs(int(arlo.read_left_wheel_encoder())),
|
|
abs(int(arlo.read_right_wheel_encoder()))
|
|
)
|
|
|
|
def test_anticlockwise(arlo, l_power, r_power):
|
|
arlo.reset_encoder_counts()
|
|
arlo.go_diff(l_power, r_power, 0, 1)
|
|
sleep(TESTING_SLEEP_TIME)
|
|
arlo.stop()
|
|
return (
|
|
abs(int(arlo.read_left_wheel_encoder())),
|
|
abs(int(arlo.read_right_wheel_encoder()))
|
|
)
|
|
|
|
class Arlo():
|
|
def __init__(self, calibration_code = None) -> None:
|
|
self.robot = Robot()
|
|
if calibration_code is None:
|
|
self._calibrate()
|
|
elif calibration_code == False:
|
|
self.calibration_code = DEFAULT_CALIBRATION_CODE
|
|
self._decode_calibration_code()
|
|
else:
|
|
self.calibration_code = calibration_code
|
|
self._decode_calibration_code()
|
|
|
|
def forward(self, distance) -> None:
|
|
left, right = tuple(self.wheel_values[0])
|
|
self.robot.go_diff(left, right, 1, 1)
|
|
sleep((distance * self.timing[0])/1000)
|
|
self.robot.stop()
|
|
|
|
def backwards(self, distance) -> None:
|
|
left, right = tuple(self.wheel_values[1])
|
|
self.robot.go_diff(left, right, 0, 0)
|
|
sleep((distance * self.timing[1])/1000)
|
|
self.robot.stop()
|
|
|
|
def clockwise(self, angle) -> None:
|
|
left, right = tuple(self.wheel_values[2])
|
|
self.robot.go_diff(left, right, 1, 0)
|
|
sleep((angle * self.timing[2])/1000)
|
|
self.robot.stop()
|
|
|
|
def anticlockwise(self, angle) -> None:
|
|
left, right = tuple(self.wheel_values[3])
|
|
self.robot.go_diff(left, right, 0, 1)
|
|
sleep((angle * self.timing[3])/1000)
|
|
self.robot.stop()
|
|
|
|
def estimate_distance(self, bounding_box):
|
|
# avg_left = (bounding_box[0][0] + bounding_box[3][0])/2
|
|
# avg_right = (bounding_box[1][0] + bounding_box[2][0])/2
|
|
# avg_up = (bounding_box[0][1] + bounding_box[1][1])/2
|
|
# avg_down = (bounding_box[2][1] + bounding_box[3][1])/2
|
|
|
|
# avg_width = avg_right - avg_left
|
|
# avg_height = avg_down - avg_up
|
|
|
|
# avg_size = (avg_width + avg_height)/2
|
|
|
|
# return (FOCAL_LENGTH * 14.5)/avg_size
|
|
|
|
return cv2.aruco.estimatePoseSingleMarkers(
|
|
np.array([bounding_box]), 14.5, CAMERA_MATRIX, DIST_COEF
|
|
)[1][0][0][2]
|
|
|
|
def find_aruco(self, image):
|
|
aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
|
|
aruco_params = cv2.aruco.DetectorParameters_create()
|
|
corners, ids, _ = cv2.aruco.detectMarkers(
|
|
image,
|
|
aruco_dict,
|
|
parameters=aruco_params
|
|
)
|
|
|
|
if corners is None:
|
|
return []
|
|
|
|
return [(box[0], ids[i]) for i, box in enumerate(corners)]
|
|
|
|
def draw_arucos(self, image, bounding_boxes):
|
|
for bounding, n in bounding_boxes:
|
|
bounding_box = [
|
|
(int(x), int(y)) for x, y in bounding.reshape((4,2))
|
|
]
|
|
print(bounding_box)
|
|
|
|
cv2.line(image, bounding_box[0], bounding_box[1], (0,255,0), 2)
|
|
cv2.line(image, bounding_box[1], bounding_box[2], (0,255,0), 2)
|
|
cv2.line(image, bounding_box[2], bounding_box[3], (0,255,0), 2)
|
|
cv2.line(image, bounding_box[3], bounding_box[0], (0,255,0), 2)
|
|
|
|
cv2.putText(
|
|
image,
|
|
str(n[0]),
|
|
(bounding_box[0][0] - 10, bounding_box[0][1] - 10),
|
|
cv2.FONT_HERSHEY_COMPLEX,
|
|
0.8,
|
|
(0,255,0),
|
|
2
|
|
)
|
|
|
|
return image
|
|
|
|
def _decode_calibration_code(self) -> None:
|
|
wheel_values_hex, timing_hex = tuple(self.calibration_code.split("_"))
|
|
self.wheel_values = [
|
|
[int(x, 16) for x in values.split(".")]
|
|
for values in wheel_values_hex.split("-")
|
|
]
|
|
self.timing = [int(x, 16) for x in timing_hex.split("-")]
|
|
|
|
def _calibrate(self) -> None:
|
|
values = [START_VALUES.copy() for _ in range(4)]
|
|
cps = [0 for _ in range(4)]
|
|
calibrated = [False for _ in range(4)]
|
|
tests = [
|
|
test_forward,
|
|
test_back,
|
|
test_clockwise,
|
|
test_anticlockwise
|
|
]
|
|
|
|
while not all(calibrated):
|
|
print(calibrated, values)
|
|
for i, function in enumerate(tests):
|
|
calibrated[i] = False
|
|
wheels = function(self.robot, *values[i])
|
|
fraction = max(wheels)/min(wheels)
|
|
if fraction <= THRESHOLD:
|
|
calibrated[i] = True
|
|
elif wheels[0] < wheels[1]:
|
|
values[i][0] += 1
|
|
else:
|
|
values[i][1] += 1
|
|
|
|
cps[i] = wheels[0]/TESTING_SLEEP_TIME
|
|
|
|
time = [0 for _ in range(4)]
|
|
|
|
cpc = 144/(3.14159*15) # wheel counts per cm
|
|
|
|
# milliseconds per 1cm forward
|
|
time[0] = int((1000 * cpc)/cps[0])
|
|
# milliseconds per 1cm backwards
|
|
time[1] = int((1000 * cpc)/cps[1])
|
|
|
|
cpr = 3.1415*38 # 1 rotation in cm
|
|
# milliseconds per 1 degrees clockwise
|
|
time[2] = int((1000 * cpr * cpc)/(cps[2] * 360))
|
|
# milliseconds per 1 degrees anticlockwise
|
|
time[3] = int((1000 * cpr * cpc)/(cps[3] * 360))
|
|
|
|
self.wheel_values = values
|
|
self.timing = time
|
|
|
|
values_hex = "-".join(
|
|
[".".join([format(i, "x") for i in v]) for v in values]
|
|
)
|
|
time_hex = "-".join([format(i, "x") for i in time])
|
|
self.calibration_code = f"{values_hex}_{time_hex}"
|
|
print("Calibration code:", self.calibration_code)
|