diff --git a/rally2.py b/rally2.py new file mode 100644 index 0000000..70fa554 --- /dev/null +++ b/rally2.py @@ -0,0 +1,69 @@ +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() \ No newline at end of file