From 7d9285e6daee8b391cfb223459024a3d7e19bda3 Mon Sep 17 00:00:00 2001
From: Nolan Mascrier <63266772+NolanMascrier@users.noreply.github.com>
Date: Wed, 5 Mar 2025 12:19:26 +0100
Subject: [PATCH] Support for 6 sensors at once

---
 ArduinoCube/ArduinoCube.ino | 150 ++++++++++++++++--------------------
 1 file changed, 65 insertions(+), 85 deletions(-)

diff --git a/ArduinoCube/ArduinoCube.ino b/ArduinoCube/ArduinoCube.ino
index a157419..69965a9 100644
--- a/ArduinoCube/ArduinoCube.ino
+++ b/ArduinoCube/ArduinoCube.ino
@@ -12,11 +12,22 @@
 */
 
 #include <Arduino_APDS9960.h>
-#include <ArduinoBLE.h>
+//#include <WiFiNINA.h>
+#include "arduino_secrets.h"
 
-#define R 2
-#define O 3
-#define V 4
+#define TRIGG_BOTTOM D12
+#define TRIGG_TOP D10
+#define TRIGG_1 D8
+#define TRIGG_2 A0
+#define TRIGG_3 A2
+#define TRIGG_4 A4
+
+#define ECHO_BOTTOM D11
+#define ECHO_TOP D9
+#define ECHO_1 D7
+#define ECHO_2 A1
+#define ECHO_3 A3
+#define ECHO_4 A5
 
 #define STATUS_SAFE 0
 #define STATUS_CLOSE 1
@@ -26,9 +37,14 @@
 #define DISTANCE_SAFE 120 
 
 int status = STATUS_SAFE;
+//int wifi_status = WL_IDLE_STATUS;
 
 int ledState = LOW;
 
+//please enter your sensitive data in the Secret tab
+char ssid[] = SECRET_SSID;                // your network SSID (name)
+char pass[] = SECRET_PASS;                // your network password (use for WPA, or use as key for WEP)
+
 unsigned long previousMillis = 0;
 
 const long intervalLong = 750;
@@ -40,85 +56,38 @@ const long intervalShort = 20;
 const int trigger = 12;
 const int echo = 11;
 
+int count = 0;
+
 long duration = 0;
 int distance = 0;
 
-BLEService ledService("180A");
-BLEByteCharacteristic switchCharacteristic("2A57", BLERead | BLEWrite);
+long durations[6];
+long distances[6];
 
 void setup() {
   Serial.begin(9600);
-  while (!Serial)
-    ;
+  Serial.println("Starting up");
 
   if (!APDS.begin()) {
     Serial.println("Error initializing APDS-9960 sensor!");
     while (1);
   }
-  if (!BLE.begin()) {
-    Serial.println("starting Bluetooth® Low Energy failed!");
-    while (1);
-  }
+  pinMode(LED_BUILTIN, OUTPUT);
 
-  BLE.setLocalName("Nano 33 BLE Sense");
-  BLE.setAdvertisedService(ledService);
-  ledService.addCharacteristic(switchCharacteristic);
-  BLE.addService(ledService);
-  switchCharacteristic.writeValue(0);
-  BLE.advertise();
+  //Init the sensors
+  pinMode(TRIGG_BOTTOM, OUTPUT);
+  pinMode(TRIGG_TOP, OUTPUT);
+  pinMode(TRIGG_1, OUTPUT);
+  pinMode(TRIGG_2, OUTPUT);
+  pinMode(TRIGG_3, OUTPUT);
+  pinMode(TRIGG_4, OUTPUT);
 
-  pinMode(trigger, OUTPUT);
-  pinMode(echo, INPUT);
-  //LEDs
-  pinMode(R, OUTPUT);
-  pinMode(O, OUTPUT);
-  pinMode(V, OUTPUT);
-  analogWrite(R, 0);
-  analogWrite(O, 0);
-  analogWrite(V, 0);
-}
-
-
-/**
- * Update the status of processor according to the
- * read distance.
- *
- * @arg distance : read distance from the sensor.
-*/
-void update_status(int distance)
-{
-  if (distance <= DISTANCE_DANGER && status != STATUS_DANGER)
-  {
-      status = STATUS_DANGER;
-      Serial.print("DANGER ! Obstacle found at distance ");
-      Serial.print(distance);
-      Serial.println("cm");
-
-      analogWrite(R, 255);
-      analogWrite(O, 0);
-      analogWrite(V, 0);
-  }
-  else if (distance > DISTANCE_DANGER &&
-      distance <= DISTANCE_SAFE && status != STATUS_CLOSE)
-  {
-      status = STATUS_CLOSE;
-      Serial.print("WARNING ! Obstacle closing on at distance ");
-      Serial.print(distance);
-      Serial.println("cm");
-
-      analogWrite(R, 0);
-      analogWrite(O, 255);
-      analogWrite(V, 0);
-  }
-  else if (distance > DISTANCE_SAFE && status != STATUS_SAFE)
-  {
-      status = STATUS_SAFE;
-      Serial.println("All is good :)");
-
-      analogWrite(R, 0);
-      analogWrite(O, 0);
-      analogWrite(V, 255);
-  }
+  pinMode(ECHO_BOTTOM, INPUT);
+  pinMode(ECHO_TOP, INPUT);
+  pinMode(ECHO_1, INPUT);
+  pinMode(ECHO_2, INPUT);
+  pinMode(ECHO_3, INPUT);
+  pinMode(ECHO_4, INPUT);
 }
 
 /**
@@ -126,9 +95,9 @@ void update_status(int distance)
  * 
  * @return calculated distance.
 */
-int read_sensor()
+long fire_sensor(int trigger, int echo)
 {
-  int dist;
+  long dist;
   long dur;
 
   digitalWrite(trigger, LOW);
@@ -136,24 +105,35 @@ int read_sensor()
   digitalWrite(trigger, HIGH);
   delayMicroseconds(10);
   digitalWrite(trigger, LOW);
-  duration = pulseIn(echo, HIGH);
-  distance = duration * 0.034 / 2;
-
+  dur = pulseIn(echo, HIGH);
+  dist = dur * 0.034 / 2;
+  delay(50);
   return (dist);
 }
 
 void loop() {
-  BLEDevice central = BLE.central();
   unsigned long currentMillis = millis();
-  digitalWrite(trigger, LOW);
-  delayMicroseconds(2);
-  digitalWrite(trigger, HIGH);
-  delayMicroseconds(10);
-  digitalWrite(trigger, LOW);
-  duration = pulseIn(echo, HIGH);
-  distance = duration * 0.034 / 2;
-  update_status(distance);
+  
+  Serial.print("Measure number ");
+  Serial.println(count);
+  distances[0] = fire_sensor(TRIGG_BOTTOM, ECHO_BOTTOM);
+  distances[1] = fire_sensor(TRIGG_TOP, ECHO_TOP);
+  distances[2] = fire_sensor(TRIGG_1, ECHO_1);
+  distances[3] = fire_sensor(TRIGG_2, ECHO_2);
+  distances[4] = fire_sensor(TRIGG_3, ECHO_3);
+  distances[5] = fire_sensor(TRIGG_4, ECHO_4);
+
+  for (int i = 0; i < 6; i++)
+  {
+    Serial.print("Sensor {");
+    Serial.print(i);
+    Serial.print("} : ");
+    Serial.print(distances[i]);
+    Serial.println("cm");
+  }
+  //update_status(distance);
 
   // wait a bit before reading again
-  delay(350);
+  delayMicroseconds(1000000);
+  count++;
 }