* moved code for straight trajectory

Phobos97 · May 7, 2022 · 331516b · 331516b
1 parent 3053d84
commit 331516b
Show file tree

Hide file tree

Showing 2 changed files with 54 additions and 58 deletions.
diff --git a/Main/straight.py b/Main/straight.py
@@ -6,6 +6,10 @@
 import os
 from pathlib import Path
 
+from picamera.array import PiRGBArray
+from picamera import PiCamera
+import pygame
+
 # modules
 script_dir = os.path.dirname(os.path.abspath(__file__))
 project_root_dir = str(Path(script_dir).parents[0])
@@ -97,15 +101,57 @@ def start(self):
         self.robot.move(0)
 
 
+def straight(mode):
+    with PiCamera() as camera:
+        camera.resolution = (640, 480)
+        camera.framerate = 24
+        rawCapture = PiRGBArray(camera, size=(640, 480))
+        time.sleep(0.5)
+
+        # initialize bouncebot
+        bb = BounceBot()
+        # initialize object detector
+        detector = ObjectDetector()
+        # initialize timer
+        timer = Timer()
+
+        try:
+            for i in range(2):
+                timer.start_timer()
+                bb.move(1)
+
+                for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True):
+                    if timer.get_timer() < distance_to_time(200):
+                        bb.move(0)
+                        break
+                    if mode == 2:
+                        obj, direction = detector.check_for_object(frame=frame, distance_to_dodge=25, show_video=True)
+                        if obj is not None:
+                            print("OBJECT DETECTED!")
+                            timer.pause_timer()
+                            bb.move(0)
+                            bb.around_obstacle(direction=direction)
+                            timer.resume_timer()
+                if i == 0:
+                    print("U-TURN")
+                    bb.u_turn()
+        finally:
+            camera.close()
+            rawCapture.close()
+            pygame.quit()
+            bb.close()
+
+
 def parse_args():
     parser = argparse.ArgumentParser()
-    parser.add_argument("--challenge_num", help="specifies which challenge the code runs on \
-                            [1-no object avoidance/2-object avoidance]", default=1)
+    parser.add_argument("--challenge_num", help="specifies which challenge the code runs on [1/2/3/4]", default=1)
     return parser.parse_args()
 
 
 if __name__ == "__main__":
     args = parse_args()
 
-    m = Manager(mode=args.challenge_num)
-    m.start()
+    # m = Manager(mode=args.challenge_num)
+    # m.start()
+
+    straight(args.challenge_num)
diff --git a/RobotControl/manual_control.py b/RobotControl/manual_control.py
@@ -1,12 +1,7 @@
-import argparse
+from bouncebot_control import BounceBot
+import pygame
 import cv2
 import numpy as np
-import pygame
-
-from plot_distance_vs_time import distance_to_time
-from Main.timer import Timer
-from ObjectDetection.object_detection import ObjectDetector
-from bouncebot_control import BounceBot
 from picamera.array import PiRGBArray
 from picamera import PiCamera
 import time
@@ -46,6 +41,7 @@ def process_directions(self, directions):
             elif i == 4:
                 self.bouncebot.rotate_turret(direction)
 
+
     def update(self):
         pygame.event.pump()
         keys = pygame.key.get_pressed()
@@ -105,47 +101,6 @@ def update(self):
         self.process_directions(directions)
 
 
-def straight(mode):
-    with PiCamera() as camera:
-        camera.resolution = (640, 480)
-        camera.framerate = 24
-        rawCapture = PiRGBArray(camera, size=(640, 480))
-        time.sleep(0.5)
-
-        # initialize bouncebot
-        bb = BounceBot()
-        # initialize object detector
-        detector = ObjectDetector()
-        # initialize timer
-        timer = Timer()
-
-        try:
-            for i in range(2):
-                timer.start_timer()
-                bb.move(1)
-
-                for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True):
-                    if timer.get_timer() < distance_to_time(2):
-                        bb.move(0)
-                        break
-                    if mode == 2:
-                        obj, direction = detector.check_for_object(frame=frame, distance_to_dodge=25, show_video=True)
-                        if obj is not None:
-                            print("OBJECT DETECTED!")
-                            timer.pause_timer()
-                            bb.move(0)
-                            bb.around_obstacle(direction=direction)
-                            timer.resume_timer()
-                if i == 0:
-                    print("U-TURN")
-                    bb.u_turn()
-        finally:
-            camera.close()
-            rawCapture.close()
-            pygame.quit()
-            bb.close()
-
-
 def main():
     # initialize camera
     with PiCamera() as camera:
@@ -183,9 +138,4 @@ def main():
 
 
 if __name__ == "__main__":
-    parser = argparse.ArgumentParser()
-    parser.add_argument("--challenge_num", help="specifies which challenge the code runs on [1/2/3/4]", default=1)
-    args = parser.parse_args()
-
-    # main()
-    straight(args.challenge_num)
+    main()