86 lines
1.7 KiB
Python
86 lines
1.7 KiB
Python
import time
|
|
import numpy as np
|
|
import cv2
|
|
|
|
import robot
|
|
|
|
LANDMARKS = [7,11,10,6]
|
|
|
|
LANDMARK_POSITIONS = {
|
|
7: [0,0],
|
|
11: [300,0],
|
|
10: [0,400],
|
|
6: [300,400]
|
|
}
|
|
|
|
POWER = 70
|
|
|
|
TURN_T = 7.9 # 1 degree
|
|
DRIVE_T = 22 # 1 centimeter
|
|
|
|
RIGHT_WHEEL_OFFSET = 4
|
|
|
|
CLOCKWISE_OFFSET = 0.96
|
|
|
|
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 look_around():
|
|
pass
|
|
|
|
def turn_towards_landmark(est_pose, landmark):
|
|
pass
|
|
|
|
def time_to_landmark(est_pose, landmark):
|
|
"""Kør indenfor 1 meter"""
|
|
pass
|
|
|
|
def drive_until_stopped(noah):
|
|
noah.go_diff(POWER, POWER+RIGHT_WHEEL_OFFSET, 1, 1)
|
|
start = time.time()
|
|
while True:
|
|
forward_dist = noah.read_front_sensor()
|
|
if forward_dist < 1000:
|
|
noah.stop()
|
|
break
|
|
|
|
end = time.time()
|
|
return end - start
|
|
|
|
def drunk_drive(noah):
|
|
pass
|
|
|
|
def inch_closer(noah):
|
|
noah.go_diff(POWER, POWER+RIGHT_WHEEL_OFFSET, 1, 1)
|
|
while True:
|
|
forward_dist = noah.read_front_sensor()
|
|
if forward_dist < 300:
|
|
noah.stop()
|
|
break
|
|
|
|
def main():
|
|
landmark_order = LANDMARKS + [
|
|
LANDMARKS[0]
|
|
]
|
|
|
|
noah = robot.Robot() # Noah er vores robots navn
|
|
|
|
for landmark in landmark_order:
|
|
while True:
|
|
est_pose = look_around()
|
|
est_pose = turn_towards_landmark(est_pose, landmark)
|
|
drive_time = time_to_landmark(est_pose, landmark)
|
|
if not abs(drive_until_stopped(noah) - drive_time) < 0.5:
|
|
drunk_drive(noah)
|
|
continue
|
|
inch_closer(noah)
|
|
break
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main() |