From e96258cdb83c2aa62f8f99fdea0e5e7780ee100b Mon Sep 17 00:00:00 2001
From: Rihards Skuja <rssk@protonmail.com>
Date: Sat, 30 Dec 2017 22:49:18 +0200
Subject: [PATCH] Use OCR0x to generate PWM for servo motors

---
 src/main.c | 51 ++++++++++++++++++---------------------------------
 1 file changed, 18 insertions(+), 33 deletions(-)

diff --git a/src/main.c b/src/main.c
index 5e7ba95..1618702 100644
--- a/src/main.c
+++ b/src/main.c
@@ -9,19 +9,25 @@
 #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_pwm(void)
 {
-    DDRB |= 1 << SERVO_L_PIN | 1 << SERVO_R_PIN;
-    /* Phase Correct PWM; Set/clear on Compare Match when down/up-counting */
-    TCCR1A |= 1 << WGM11 | 1 << COM1A1 | 1 << COM1B1;
-    /*  "Clear Timer on Compare match" mode; Prescaler = 1 */
-    TCCR1B |= 1 << WGM12 | 1 << WGM13 | 1 << CS10;
-    ICR1 = F_CPU / 50 - 1; // 50Hz required by servos
+    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;
+    DDRD |= 1 << LED_2;
 }
 
 void init_adc(void)
@@ -38,7 +44,7 @@ void read_temp(void)
     sei();
     for (;;) {
         d = ds18b20_gettemp();
-        if (d >= 21)
+        if (d >= 23)
             SET(PORTD, LED_1);
         else
             CLR(PORTD, LED_1);
@@ -49,20 +55,9 @@ void read_temp(void)
 
 void run_servos(void)
 {
-    /* Stop */
-    SERVO_L = 1500;
-    SERVO_R = 1500;
-    _delay_ms(1500);
-
-    /* Reverse */
-    SERVO_L = 2000;
-    SERVO_R = 2000;
-    _delay_ms(1500);
-
-    /* Forwards */
-    SERVO_L = 1000;
-    SERVO_R = 1000;
-    _delay_ms(1500);
+    SERVO_L = US2TIMER0(SERVO_MAX);
+    SERVO_R = US2TIMER0(SERVO_MID);
+    _delay_ms(10000); // 10s
 }
 
 uint16_t read_adc(uint8_t channel)
@@ -77,22 +72,12 @@ uint16_t read_adc(uint8_t channel)
 
 int main(void)
 {
-    uint16_t adc_val;
-
     init_led();
     init_pwm();
     init_adc();
 
-    while (1) {
-        adc_val = read_adc(0);
-        if (adc_val <= 512) {
-            SERVO_R = 2000;
-            SERVO_L = 2000;
-        } else {
-            SERVO_R = 1000;
-            SERVO_L = 1000;
-        }
-    }
+    run_servos();
+    read_temp();
 
     return 0;
 }