From 700d568e662568f2ce3acc1e60f07e2b09a2eb2b Mon Sep 17 00:00:00 2001
From: "[tiffany" <tiffany2626lin@gmail.com>
Date: Sun, 1 Jan 2023 17:00:57 +0100
Subject: [PATCH] Update final_project.ino

---
 final_project/final_project.ino | 71 ++++++++++++++++++++-------------
 1 file changed, 44 insertions(+), 27 deletions(-)

diff --git a/final_project/final_project.ino b/final_project/final_project.ino
index 0a46366..67addba 100644
--- a/final_project/final_project.ino
+++ b/final_project/final_project.ino
@@ -3,8 +3,8 @@
  *
  * by Clemence, Romaric & Tiffany 
  *
- * This code will find the shortest path from point A to B
- * in a 6x6 grid while avoiding obstacles placed on the grid.
+ * This robot will find the shortest path from point A to B
+ * in a 8x8 grid while avoiding obstacles placed on the grid.
  * 
  */
 
@@ -28,8 +28,21 @@
 #define ECHO_PIN     11  
 #define MAX_DISTANCE 200
 
+//setting up time constants for avoiding obstacles
+#define OBSTACLE_CHECK_INTERVAL 500 // check for obstacles every 500 ms
+#define OBSTACLE_AVOID_DURATION 2000 // avoid obstacles for 2 sec
+
 NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
 
+//setting up graph 
+struct GridNode {
+  int x;
+  int y;
+  std::vector<GridNode*> neighbors;
+  int cost;
+  bool isObstacle;
+};
+
 //setting up Zumo robot
 ZumoBuzzer buzzer;
 ZumoReflectanceSensorArray reflectanceSensors;
@@ -47,30 +60,34 @@ void setup() {
 
 /*************************************************/
 
-void loop() 
-{
-  //learning how ultrasonic sensor works
-  delay(200);                      // pause 50ms between pings (about 20 pings/sec)
-  unsigned int uS = sonar.ping(); // this int gets ping time in microseconds (uS) after sending
-  Serial.print("Ping: ");
-  Serial.print(uS); // Convert ping time to distance in cm and print result (0 = outside set distance range)
-  Serial.println("qdssd");
-  if (uS != 0 && uS < 500){
-     Serial.println("hey");
-    motors.setSpeeds(STOP, STOP);
-    // if (random(2)==0)
-    // {
-    //   motors.setSpeeds(-REVERSE_SPEED, REVERSE_SPEED);
-    // }
-    // else
-    // {
-    //   motors.setSpeeds(REVERSE_SPEED, -REVERSE_SPEED);
-    // }
-    //delay(2000);
-  } else {
-     Serial.println("yo");
-    motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED);
-  }
 
-  
+void loop(){
+  static unsigned long lastObstacleCheck = 0;
+  static boolean avoidingObstacle = false;
+
+  // Check for obstacles every OBSTACLE_CHECK_INTERVAL milliseconds
+  if (millis() - lastObstacleCheck >= OBSTACLE_CHECK_INTERVAL) {
+    lastObstacleCheck = millis();
+
+    // Measure distance to nearest obstacle
+    unsigned int uS = sonar.ping();
+    Serial.print(uS);
+
+    // If an obstacle is detected and we're not already avoiding it, start avoiding it
+    if (uS != 0 && uS < MAX_DISTANCE && !avoidingObstacle) {
+      avoidingObstacle = true;
+      motors.setSpeeds(-REVERSE_SPEED, -REVERSE_SPEED);
+      delay(REVERSE_DURATION);
+      motors.setSpeeds(STOP, STOP);
+      delay(STOP_DURATION);
+
+      // Choose a random direction to turn
+      if (random(2) == 0) {
+        motors.setSpeeds(-TURN_SPEED, TURN_SPEED);
+      } else {
+        motors.setSpeeds(TURN_SPEED, -TURN_SPEED);
+      }
+
+  }
+  }
 }