From 096b9f68da43da6376bccd54a8fdb59038e2ba09 Mon Sep 17 00:00:00 2001
From: WillB97 <will@rudi.org.uk>
Date: Sat, 31 Aug 2024 23:01:46 +0100
Subject: [PATCH] Update arduino firmware file to support ultrasound

---
 resources/kit/arduino-fw.ino | 34 +++++++++++++++++++++++++++++++++-
 1 file changed, 33 insertions(+), 1 deletion(-)

diff --git a/resources/kit/arduino-fw.ino b/resources/kit/arduino-fw.ino
index b18c07bc..06ef0b49 100644
--- a/resources/kit/arduino-fw.ino
+++ b/resources/kit/arduino-fw.ino
@@ -3,13 +3,15 @@
 // We communicate with the Arduino at 115200 baud.
 #define SERIAL_BAUD 115200
 
-#define FW_VER 1
+#define FW_VER 2
 
 void setup() {
   Serial.begin(SERIAL_BAUD);
 }
 
 int read_pin() {
+  // Convert the ASCII character to a pin number.
+  // a -> 0, b -> 1, c -> 2, etc.
   while (!Serial.available());
   int pin = Serial.read();
   return (int)(pin - 'a');
@@ -43,6 +45,33 @@ void command_mode(int mode) {
   pinMode(pin, mode);
 }
 
+void command_ultrasound() {
+  int pulse = read_pin();
+  int echo = read_pin();
+
+  // config pins to correct modes
+  pinMode(pulse, OUTPUT);
+  pinMode(echo, INPUT);
+
+  // provide pulse to trigger reading
+  digitalWrite(pulse, LOW);
+  delayMicroseconds(2);
+  digitalWrite(pulse, HIGH);
+  delayMicroseconds(5);
+  digitalWrite(pulse, LOW);
+
+  // measure the echo time on the echo pin
+  int duration = pulseIn(echo, HIGH, 60000);
+  Serial.print(microsecondsToMm(duration));
+}
+
+long microsecondsToMm(long microseconds) {
+  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
+  // The ping travels out and back, so to find the distance we need half
+  // 10 x (us / 29 / 2)
+  return (5 * microseconds / 29);
+}
+
 void loop() {
   // Fetch all commands that are in the buffer
   while (Serial.available()) {
@@ -70,6 +99,9 @@ void loop() {
       case 'p':
         command_mode(INPUT_PULLUP);
         break;
+      case 'u':
+        command_ultrasound();
+        break;
       case 'v':
         Serial.print("SRcustom:");
         Serial.print(FW_VER);