Merge pull request #2 from SuragNuthulapaty/new_branch

More Coding

.gitignore

@@ -1,3 +1,8 @@
 .DS_Store
 Data/
-__pycache__/
+__pycache__/
+*.dylib
+yolov8n.pt
+venv
+__pycache__/
+

code/.gitignore

@@ -0,0 +1 @@
+*.dylib

code/full.py

@@ -1,19 +1,16 @@
-from functools import total_ordering
-from gpiozero import Motor, DistanceSensor
-from time import sleep
-import numpy as np
 from src import motor as motor_module
 from src import motor_rotations
 from src import led as led_module
 import time
 from video import detectPersonInFrame, ModelType
 from src import distance_sensor as distance_sensor_module
-import cv2
 from src import camera as camera_module
-
+from src import switch as switch_module
 
 MOTOR_OFFSET = 1
-DT = 0.25
+DT = 0.1
+DISTANCE_RANGE = 0.7
+SQUARE_SIZE = 8
 
 #first we need to start the timer at the start of the program
 #the normal behaviour is that the robot will move forward for 10 seconds and then take a 90 degree turn and it will keep doing this so it moves in a square
@@ -25,27 +22,133 @@
 
 def move_in_square():
     global start_time
+    pic_time = 0
     while True:
-        while time.time() - start_time < 10:
+        while time.time() - start_time < SQUARE_SIZE:
 
             distance1 = distance_sensor1.distance
             distance2 = distance_sensor2.distance
 
-            if 0 < distance1 < 0.2 or 0 < distance2 < 0.2:
+            if 0 < distance1 < DISTANCE_RANGE or 0 < distance2 < DISTANCE_RANGE:
+                led1.on() #detected something and now we will turn towards it
+                temp_start_time = time.time()
+                handle_obstacle(distance1, distance2)
+                start_time += (time.time() - temp_start_time)
+
+            led1.off()
+            motor_rotations.move_forward(left_motor, right_motor, 1, s1=1, s2=1)
+            time.sleep(DT)
+
+            temp_start_time = time.time()
+            camera.capture()
+            if pic_time%4 == 0:
+                image_array = camera.image_array
+                person_detected = detectPersonInFrame(image_array[::-1, :, :3], ModelType.YOLOv8n)
+                start_time += (time.time() - temp_start_time)
+
+            if person_detected == 1:
                 led1.on()
+                led2.on()
+                time.sleep(2)
+                led1.off()
+                led2.off()
+                start_time+=2
+
+            person_detected = 0
+            pic_time += 1
+
+        led2.on()
+        motor_rotations.rotate_cw_90_deg(left_motor, right_motor)
+        start_time = time.time()
+        time.sleep(0.5)
+        led2.off()
+
+def move_in_line():
+    global start_time
+    pic_time = 0
+    while True:
+        while time.time() - start_time < SQUARE_SIZE:
+
+            distance1 = distance_sensor1.distance
+            distance2 = distance_sensor2.distance
+
+            if 0 < distance1 < DISTANCE_RANGE or 0 < distance2 < DISTANCE_RANGE:
+                led1.on() #detected something and now we will turn towards it
                 temp_start_time = time.time()
                 handle_obstacle(distance1, distance2)
                 start_time += (time.time() - temp_start_time)
 
             led1.off()
             motor_rotations.move_forward(left_motor, right_motor, 1, s1=1, s2=1)
-            sleep(DT)
+            time.sleep(DT)
+
+            temp_start_time = time.time()
+            camera.capture()
+            if pic_time%4 == 0:
+                image_array = camera.image_array
+                person_detected = detectPersonInFrame(image_array[::-1, :, :3], ModelType.YOLOv8n)
+                start_time += (time.time() - temp_start_time)
+
+            if person_detected == 1:
+                led1.on()
+                led2.on()
+                time.sleep(2)
+                led1.off()
+                led2.off()
+                start_time+=2
+
+            person_detected = 0
+            pic_time += 1
 
         led2.on()
-        stop()
+        motor_rotations.rotate_cw_90_deg(left_motor, right_motor)
+        time.sleep(0.5)
         motor_rotations.rotate_cw_90_deg(left_motor, right_motor)
         start_time = time.time()
-        sleep(4)
+        time.sleep(0.5)
+        led2.off()
+
+def move_in_triangle():
+    global start_time
+    pic_time = 0
+    while True:
+        while time.time() - start_time < SQUARE_SIZE:
+
+            distance1 = distance_sensor1.distance
+            distance2 = distance_sensor2.distance
+
+            if 0 < distance1 < DISTANCE_RANGE or 0 < distance2 < DISTANCE_RANGE:
+                led1.on() #detected something and now we will turn towards it
+                temp_start_time = time.time()
+                handle_obstacle(distance1, distance2)
+                start_time += (time.time() - temp_start_time)
+
+            led1.off()
+            motor_rotations.move_forward(left_motor, right_motor, 1, s1=1, s2=1)
+            time.sleep(DT)
+
+            temp_start_time = time.time()
+            camera.capture()
+            if pic_time%4 == 0:
+                image_array = camera.image_array
+                person_detected = detectPersonInFrame(image_array[::-1, :, :3], ModelType.YOLOv8n)
+                start_time += (time.time() - temp_start_time)
+
+            if person_detected == 1:
+                led1.on()
+                led2.on()
+                time.sleep(2)
+                led1.off()
+                led2.off()
+                start_time+=2
+
+            person_detected = 0
+            pic_time += 1
+
+        led2.on()
+        motor_rotations.rotate_cw_120_deg(left_motor, right_motor)
+        start_time = time.time()
+        time.sleep(0.5)
         led2.off()
 
 
@@ -54,7 +157,6 @@ def stop():
     right_motor.stop()
 
 def handle_obstacle(distance1, distance2):
-    stop()
     priority_distance = min(distance1, distance2)
     if distance1 == priority_distance:
         motor_rotations.rotate_ccw_90_deg(left_motor, right_motor)
@@ -63,18 +165,28 @@ def handle_obstacle(distance1, distance2):
 
     camera.capture()
     image_array = camera.image_array
-    person_detected = detectPersonInFrame(image_array[:, :, :3], ModelType.YOLOv8n)
+    person_detected = detectPersonInFrame(image_array[::-1, :, :3], ModelType.YOLOv8n)
     if person_detected:
         # led1.on()
-        # led2.on()
-        time.sleep(3)
+        led2.on()
+        time.sleep(1)
         if distance1 == priority_distance:
+            motor_rotations.move_forward(left_motor, right_motor, 1, s1=1, s2=1*MOTOR_OFFSET)
+            #time.sleep(3)
+            motor_rotations.move_backward(left_motor, right_motor, 1, s1=1, s2=1*MOTOR_OFFSET)
             motor_rotations.rotate_cw_90_deg(left_motor, right_motor)
+
+
         else:
+            motor_rotations.move_forward(left_motor, right_motor, 1, s1=1, s2=1*MOTOR_OFFSET)
+            #time.sleep(3)
+            motor_rotations.move_backward(left_motor, right_motor, 1, s1=1, s2=1*MOTOR_OFFSET)
             motor_rotations.rotate_ccw_90_deg(left_motor, right_motor)
+
+
 
         # led1.off()
-        # led2.off()
+        led2.off()
     else:
         if distance1 == priority_distance:
             motor_rotations.rotate_cw_90_deg(left_motor, right_motor)
@@ -123,6 +235,53 @@ def handle_obstacle(distance1, distance2):
             "trigger": 27
         }
     })
+
+    switch1 = switch_module.Switch({
+        "pin": 2
+    })
+
+    switch2 = switch_module.Switch({
+        "pin": 3
+    })
+
+    num_b2_press = 0
+
+    while not switch1.is_pressed:
+        print("sleeping", num_b2_press)
+        if switch2.is_pressed:
+            num_b2_press+=1
+
+        time.sleep(2)
 
+    led1.on()
+    led2.on()
+    time.sleep(2)
+
+    match num_b2_press%3:
+        case 0:
+            led1.off()
+            led2.off()
+        case 1:
+            led1.off()
+            led2.on()
+        case 2:
+            led1.on()
+            led2.off()
+
+    temp_start = time.time()
+
+    while time.time()-temp_start < 5:
+        time.sleep(1)
+
+    led1.off()
+    led2.off()
+
     start_time = time.time()
-    move_in_square()
+
+    match num_b2_press%3:
+        case 0:
+            move_in_square()
+        case 1:
+            move_in_line()
+        case 2:
+            move_in_triangle()