From a48b71620f3d535a55cc152786319b7bc4415fe8 Mon Sep 17 00:00:00 2001
From: Rihards Skuja <rssk@protonmail.com>
Date: Tue, 9 Jan 2018 20:17:50 +0200
Subject: [PATCH] Port naming, turn on both LEDs on startup

---
 include/led.h    |  7 ++++---
 include/servos.h | 10 +++++-----
 src/led.c        |  4 ++--
 src/main.c       |  4 ++++
 src/sonar.c      | 12 ++++++------
 5 files changed, 21 insertions(+), 16 deletions(-)

diff --git a/include/led.h b/include/led.h
index a22eff6..d2ad6eb 100644
--- a/include/led.h
+++ b/include/led.h
@@ -1,9 +1,10 @@
 #ifndef LED_H
 #define LED_H
 
-#define LED_DDR     DDRD
-#define LED_1_BIT   PD3
-#define LED_2_BIT   PD4
+#define LED_DDR         DDRD
+#define LED_PORT        PORTD
+#define LED_GREEN_BIT   PD3
+#define LED_BLUE_BIT    PD4
 
 void init_leds(void);
 
diff --git a/include/servos.h b/include/servos.h
index e316410..fa0b899 100644
--- a/include/servos.h
+++ b/include/servos.h
@@ -1,11 +1,11 @@
 #ifndef SERVOS_H
 #define SERVOS_H
 
-#define SERVO_DDR DDRD
-#define SERVO_L_REG OCR0B
-#define SERVO_R_REG OCR0A
-#define SERVO_L_BIT PD5
-#define SERVO_R_BIT PD6
+#define SERVO_DDR       DDRD
+#define SERVO_L_REG     OCR0B
+#define SERVO_R_REG     OCR0A
+#define SERVO_L_BIT     PD5
+#define SERVO_R_BIT     PD6
 
 #define SERVO_MIN       930 // (us). Go backwards
 #define SERVO_MAX       1930 // (us). Go forwards
diff --git a/src/led.c b/src/led.c
index 4a2b702..1057fd4 100644
--- a/src/led.c
+++ b/src/led.c
@@ -4,6 +4,6 @@
 
 void init_leds(void)
 {
-    SET(LED_DDR, LED_1_BIT);
-    SET(LED_DDR, LED_2_BIT);
+    SET(LED_DDR, LED_GREEN_BIT);
+    SET(LED_DDR, LED_BLUE_BIT);
 }
diff --git a/src/main.c b/src/main.c
index d9ccd0b..e9623a7 100644
--- a/src/main.c
+++ b/src/main.c
@@ -39,6 +39,10 @@ int main(void)
     init_servos();
     init_sonar();
 
+    // 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();
 
diff --git a/src/sonar.c b/src/sonar.c
index e25d684..0746982 100644
--- a/src/sonar.c
+++ b/src/sonar.c
@@ -22,12 +22,6 @@ void trigger_sonar(void)
     _delay_us(1);
 }
 
-ISR(TIMER1_OVF_vect)
-{
-    ++overflow_counter;
-    TCNT1 = 0;
-}
-
 int32_t read_sonar(void)
 {
     int dist_in_cm = 0;
@@ -58,3 +52,9 @@ int32_t read_sonar(void)
 
     return dist_in_cm;
 }
+
+ISR(TIMER1_OVF_vect)
+{
+    ++overflow_counter;
+    TCNT1 = 0;
+}