69 lines
1.2 KiB
Python
69 lines
1.2 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(est_pose):
|
|
pass
|
|
|
|
def inch_closer():
|
|
pass
|
|
|
|
def main():
|
|
landmark_order = LANDMARKS + [
|
|
LANDMARKS[0]
|
|
]
|
|
|
|
noah = robot.Robot()
|
|
|
|
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 drive_until_stopped(drive_time) == drive_time:
|
|
# drive around
|
|
continue
|
|
inch_closer()
|
|
break
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main() |