From f40fd4b0a0cf96ed4a0b81a61375a9bf38f23c6a Mon Sep 17 00:00:00 2001
From: zwarenelle <tomie121@gmail.com>
Date: Thu, 13 Jun 2024 22:21:04 +0200
Subject: [PATCH] Added ultrasonic code

---
 src/Client/RobotController/Ultrasonic.cs | 61 ++++++++++++++++++++----
 1 file changed, 52 insertions(+), 9 deletions(-)

diff --git a/src/Client/RobotController/Ultrasonic.cs b/src/Client/RobotController/Ultrasonic.cs
index 5050dd4..2b425af 100644
--- a/src/Client/RobotController/Ultrasonic.cs
+++ b/src/Client/RobotController/Ultrasonic.cs
@@ -1,21 +1,64 @@
 using System;
-using System.IO.Ports;
 using System.Threading;
+using System.Device.Gpio;
+using System.Diagnostics;
 
-namespace SerialReadTest
+namespace RobotController
 {
-  class SerialRead
+  class Ultrasonic
   {
-    static void Main(string[] args)
+    private int ECHOPIN { get; set; }
+    private int TRIGGERPIN { get; set; }
+
+    private GpioPin echoPin;
+    private GpioPin triggerPin;
+
+    private GpioController gpio;
+
+    Ultrasonic(int ECHO, int TRIGGER)
     {
-      Console.WriteLine("Serial read init");
-      SerialPort port = new SerialPort("COM1", 9600, Parity.None, 8, StopBits.One);
-      port.Open();
-      while (true)
+      this.ECHOPIN = ECHO;
+      this.TRIGGERPIN = TRIGGER;
+
+      this.gpio = new GpioController();
+    }
+    public void Initialize()
+    {
+      // Set trigger pin as output and set it high
+      this.gpio.OpenPin(this.TRIGGERPIN, PinMode.Output);
+      this.gpio.Write(this.TRIGGERPIN, PinValue.High);
+
+      // Set echo pin as input
+      this.gpio.OpenPin(this.ECHOPIN, PinMode.Input);
+
+      Thread.Sleep(500); // delay 500ms
+    }
+    public int MeasurePulseWidth(int pin, PinValue level)
+    {
+      Stopwatch stopwatch = new Stopwatch();
+
+      // Wait for the signal to go high
+      while (this.gpio.Read(pin) != PinValue.High)
       {
-        Console.WriteLine(port.ReadLine());
+        // timeout after 50ms
+        if (stopwatch.ElapsedMilliseconds > 50)
+          return 50000;
       }
 
+      stopwatch.Start();
+
+      // Wait for the signal to go low
+      while (this.gpio.Read(pin) != PinValue.Low)
+      {
+        // timeout after 50ms
+        if (stopwatch.ElapsedMilliseconds > 50)
+          return 50000;
+      }
+
+      stopwatch.Stop();
+
+      return ((int)stopwatch.Elapsed.TotalMilliseconds * 1000) / 50; // convert to microseconds / 50 = cm
     }
+
   }
 }