From a897a71eac0049781c1f7da6c8a3a12c8ba8d2d0 Mon Sep 17 00:00:00 2001
From: Rihards Skuja <rssk@protonmail.com>
Date: Wed, 17 Jan 2018 21:56:48 +0200
Subject: [PATCH] Simplify sonar usage

---
 include/sonar.h | 16 ++++++---------
 src/main.c      | 14 ++++++++++---
 src/sonar.c     | 54 ++++++++++++++++++++++---------------------------
 3 files changed, 41 insertions(+), 43 deletions(-)

diff --git a/include/sonar.h b/include/sonar.h
index 1e42daa..8d22b48 100644
--- a/include/sonar.h
+++ b/include/sonar.h
@@ -15,21 +15,17 @@
 #define ECHO_PIN    PINC
 #define ECHO_BIT    PC5
 
-#define SPEED_OF_SOUND      343 // (m/s)
-#define US_PER_CM           58 // Time for sound to travel distance of 1cm
+#define SPEED_OF_SOUND      343 // 20°C dry air (m/s)
 #define MAX_SONAR_RANGE     8 // Trigger + echo (m)
-#define DELAY_BETWEEN_TESTS	500 // Timeout for return signal
+#define SONAR_DELAY	        60 // Timeout for return signal
 #define TIMER_MAX           65535 // For 16-bit timer
+#define CYCLES_PER_US       (F_CPU / 1000000)
+#define SONAR_TIMEOUT 	    (F_CPU * MAX_SONAR_RANGE) / SPEED_OF_SOUND
 
-#define CYCLES_PER_US   F_CPU / 1000000
-#define SONAR_TIMEOUT 	(F_CPU * MAX_SONAR_RANGE) / SPEED_OF_SOUND
-
-#define TRIG_ERROR      -1
-#define ECHO_ERROR      -2
+#define SONAR_ERROR 0
 
 
 void init_sonar(void);
-void trigger_sonar(void);
-int32_t read_sonar(void);
+uint16_t read_sonar(void);
 
 #endif /* SONAR_H */
diff --git a/src/main.c b/src/main.c
index 4a2bd38..cd22275 100644
--- a/src/main.c
+++ b/src/main.c
@@ -36,10 +36,18 @@ int main(void)
 
     // Use green LED as power LED
     SET(LED_PORT, LED_GREEN_BIT);
-    SET(LED_PORT, LED_BLUE_BIT); // Temporary for debug
 
-    run_servos();
-    read_temp();
+    /* run_servos(); */
+    /* read_temp(); */
+    while (1) {
+        uint16_t distance_int_cm = read_sonar();
+
+        if (distance_int_cm <= 10 && distance_int_cm != SONAR_ERROR)
+            SET(LED_PORT, LED_BLUE_BIT);
+        else
+            CLR(LED_PORT, LED_BLUE_BIT);
+        _delay_ms(SONAR_DELAY);
+    }
 
     return 0;
 }
diff --git a/src/sonar.c b/src/sonar.c
index 0746982..f5bd653 100644
--- a/src/sonar.c
+++ b/src/sonar.c
@@ -2,9 +2,10 @@
 #include <util/delay.h>
 #include "sonar.h"
 
-volatile uint32_t overflow_counter = 0;
-volatile uint32_t timeout_val = SONAR_TIMEOUT;
-volatile uint32_t no_of_cycles = 0;
+#include "led.h"
+
+volatile uint32_t trig_counter;
+volatile uint32_t no_of_ticks;
 
 void init_sonar(void)
 {
@@ -12,49 +13,42 @@ void init_sonar(void)
     CLR(ECHO_DDR, ECHO_BIT);
 }
 
-void trigger_sonar(void)
+uint16_t read_sonar(void)
 {
-    CLR(TRIG_PORT, TRIG_BIT);
-    _delay_us(1);
+    // Hold high for 10us while TRIGGER sends 40KHz burst
     SET(TRIG_PORT, TRIG_BIT);
-    _delay_us(12);
+    _delay_us(10);
     CLR(TRIG_PORT, TRIG_BIT);
-    _delay_us(1);
-}
 
-int32_t read_sonar(void)
-{
-    int dist_in_cm = 0;
-    trigger_sonar();
-
-    // While echo pin is low
+    trig_counter = SONAR_TIMEOUT;
+    // Wait for ECHO to receive TRIGGER burst
     while (!(CHK(ECHO_PIN, ECHO_BIT))) {
-        if (--timeout_val == 0)
-            return TRIG_ERROR; // Received no response from echo
+        if (--trig_counter == 0) {
+            return SONAR_ERROR;
+        }
     }
 
     TCNT1 = 0;
-    SET(TCCR1B, CS10);
-    SET(TIMSK1, TOIE1);
-    overflow_counter = 0;
+    SET(TCCR1B, CS10); // Prescaler = 1
+    SET(TIMSK1, TOIE1); // Enable overflow interrupts
+    no_of_ticks = 0; // Reset overflow counter
+
     sei();
-
-    // While echo pin is high
+    // Count cycles until ECHO goes low
     while (CHK(ECHO_PIN, ECHO_BIT)) {
-        if (((overflow_counter * TIMER_MAX) + TCNT1) > SONAR_TIMEOUT)
-            return ECHO_ERROR; // No echo within sonar range
+        if (no_of_ticks + TCNT1 > SONAR_TIMEOUT) {
+            return SONAR_ERROR;
+        }
     }
-
     TCCR1B = 0x00;
     cli();
-    no_of_cycles = ((overflow_counter * TIMER_MAX) + TCNT1);
-    dist_in_cm = (no_of_cycles / (US_PER_CM * CYCLES_PER_US));
 
-    return dist_in_cm;
+    no_of_ticks += TCNT1;
+
+    return (uint16_t)(no_of_ticks / (CYCLES_PER_US * 58));
 }
 
 ISR(TIMER1_OVF_vect)
 {
-    ++overflow_counter;
-    TCNT1 = 0;
+    no_of_ticks += TIMER_MAX;
 }