From 3fc7ad4c8d5ce7ca6094f895f72b40e698c50016 Mon Sep 17 00:00:00 2001
From: Rihards Skuja <rssk@protonmail.com>
Date: Tue, 2 Jan 2018 14:02:42 +0200
Subject: [PATCH] Split servo functionality in seperate headers/sources

---
 include/io.h     |  6 ------
 include/main.h   |  8 --------
 include/servos.h | 21 +++++++++++++++++++++
 src/main.c       | 20 +++-----------------
 src/servos.c     | 28 ++++++++++++++++++++++++++++
 5 files changed, 52 insertions(+), 31 deletions(-)
 create mode 100644 include/servos.h
 create mode 100644 src/servos.c

diff --git a/include/io.h b/include/io.h
index 67186db..4699712 100644
--- a/include/io.h
+++ b/include/io.h
@@ -7,10 +7,4 @@
 #define LED_1 PD3
 #define LED_2 PD4
 
-/* Servo motors */
-#define SERVO_DDR DDRD
-#define SERVO_L_PIN PD5
-#define SERVO_L OCR0B
-#define SERVO_R_PIN PD6
-#define SERVO_R OCR0A
 #endif
diff --git a/include/main.h b/include/main.h
index 5743225..adf9394 100644
--- a/include/main.h
+++ b/include/main.h
@@ -11,16 +11,8 @@
 #include "debug.h"
 #endif /* DEBUG */
 
-#define SERVO_MIN       930 // (us). Go backwards
-#define SERVO_MAX       1930 // (us). Go forwards
-#define SERVO_MID       (SERVO_MIN + SERVO_MAX) / 2 // Stop
-#define PWM_PERIOD      16384 // (1000000 / F_CPU) * PRESCALER * 256) / F_CPU (us)
-#define US2TIMER0(us)   (255 * (uint32_t)us) / PWM_PERIOD
-
 void init_led(void);
-void init_pwm(void);
 void read_temp(void);
-void run_servos(void);
 uint16_t read_adc(uint8_t channel);
 
 #endif
diff --git a/include/servos.h b/include/servos.h
new file mode 100644
index 0000000..e316410
--- /dev/null
+++ b/include/servos.h
@@ -0,0 +1,21 @@
+#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_MIN       930 // (us). Go backwards
+#define SERVO_MAX       1930 // (us). Go forwards
+#define SERVO_MID       (SERVO_MIN + SERVO_MAX) / 2 // Stop
+#define PWM_PERIOD      16384 // (1000000 / F_CPU) * PRESCALER * 256) / F_CPU (us)
+
+#define US2TIMER0(us)   (255 * (uint32_t)us) / PWM_PERIOD
+
+
+void init_servos(void);
+void run_servos(void);
+
+#endif /* SERVOS_H */
diff --git a/src/main.c b/src/main.c
index 04022a2..b3a4e61 100644
--- a/src/main.c
+++ b/src/main.c
@@ -2,18 +2,11 @@
 #include <avr/interrupt.h>
 #include <util/delay.h>
 #include "ds18b20.h"
+#include "servos.h"
+#include "sonar.h"
 #include "main.h"
 #include "io.h"
 
-void init_pwm(void)
-{
-    SERVO_DDR |= _BV(SERVO_L_PIN) | _BV(SERVO_R_PIN);
-    TCCR0A |= _BV(COM0A1) | // Clear OC0A on Compare Match, set OC0A at BOTTOM
-              _BV(COM0B1) | // Clear OC0B on Compare Match, set OC0B at BOTTOM
-              _BV(WGM00) | _BV(WGM01); // Fast PWM, 0xFF TOP
-    TCCR0B |= _BV(CS00) | _BV(CS01); // clkPWM = clkIO / 64
-}
-
 void init_led(void)
 {
     DDRD |= 1 << LED_1;
@@ -43,13 +36,6 @@ void read_temp(void)
     }
 }
 
-void run_servos(void)
-{
-    SERVO_L = US2TIMER0(SERVO_MAX);
-    SERVO_R = US2TIMER0(SERVO_MID);
-    _delay_ms(10000); // 10s
-}
-
 uint16_t read_adc(uint8_t channel)
 {
     ADMUX &= 0xF0;
@@ -63,7 +49,7 @@ uint16_t read_adc(uint8_t channel)
 int main(void)
 {
     init_led();
-    init_pwm();
+    init_servos();
     init_adc();
 
     run_servos();
diff --git a/src/servos.c b/src/servos.c
new file mode 100644
index 0000000..60257e2
--- /dev/null
+++ b/src/servos.c
@@ -0,0 +1,28 @@
+#include <avr/io.h>
+#include <util/delay.h>
+#include "common.h"
+#include "servos.h"
+
+void init_servos(void)
+{
+    SET(SERVO_DDR, SERVO_L_BIT);
+    SET(SERVO_DDR, SERVO_R_BIT);
+
+    SET(TCCR0A, COM0A1); // Clear OC0A on Compare Match, set OC0A at BOTTOM
+    SET(TCCR0A, COM0B1); // Clear OC0B on Compare Match, set OC0B at BOTTOM
+
+    // Fast PWM, 0xFF TOP
+    SET(TCCR0A, WGM00);
+    SET(TCCR0A, WGM01);
+
+    // Prescaler. clkPWM = clkIO / 64
+    SET(TCCR0B, CS00);
+    SET(TCCR0B, CS01);
+}
+
+void run_servos(void)
+{
+    SERVO_L_REG = US2TIMER0(SERVO_MAX);
+    SERVO_R_REG = US2TIMER0(SERVO_MID);
+    _delay_ms(10000); // 10s
+}