From a470de0d79fc92afb0e15cfc03de0c6a23eb67fd Mon Sep 17 00:00:00 2001
From: Rihards Skuja <rssk@protonmail.com>
Date: Sat, 25 Nov 2017 13:37:16 +0200
Subject: [PATCH] Use TC1 to drive an additional servo motor

---
 include/io.h   |  6 ++++--
 include/main.h |  3 ++-
 src/main.c     | 35 ++++++++++++++++++++---------------
 3 files changed, 26 insertions(+), 18 deletions(-)

diff --git a/include/io.h b/include/io.h
index 013225e..ee1d3d6 100644
--- a/include/io.h
+++ b/include/io.h
@@ -7,8 +7,10 @@
 #define LED_1 PD2
 
 /* Servo motors */
-#define SERVO_L PB1
-#define SERVO_R PD6
+#define SERVO_L_PIN PB1
+#define SERVO_L OCR1A
+#define SERVO_R_PIN PB2
+#define SERVO_R OCR1B
 
 /* Photo sensors */
 #define PH_1 PC1
diff --git a/include/main.h b/include/main.h
index 792efdc..326e8cc 100644
--- a/include/main.h
+++ b/include/main.h
@@ -6,7 +6,8 @@
 #define CHK(PORT, PIN) (PORT & (1 << PIN))
 #define TOG(PORT, PIN) (PORT ^= (1 << PIN))
 
-void initIO(void);
+void initLED(void);
+void initPWM(void);
 void blinkLed(void);
 void readTemp(void);
 void readPhoto(void);
diff --git a/src/main.c b/src/main.c
index 41b8260..912653f 100644
--- a/src/main.c
+++ b/src/main.c
@@ -9,21 +9,22 @@
 #include "debug.h"
 #endif /* DEBUG */
 
-void initIO()
+void initPWM(void)
 {
-    /* Servos */
-    SET(DDRB, SERVO_L);
-    /* Phase Correct PWM, 9-bit; Inverting mode */
-    TCCR1A |= 1 << WGM11 | 1 << COM1A1 | 1 << COM1A0;
+    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 << WGM13 | 1 << WGM12 | 1 << CS10;
-    ICR1 = 19999; // F_CPU / 50Hz - 1
+    TCCR1B |= 1 << WGM12 | 1 << WGM13 | 1 << CS10;
+    ICR1 = F_CPU / 50 - 1; // 50Hz required by servos
+}
 
-    /* LED */
+void initLED(void)
+{
     SET(DDRD, LED_1);
 }
 
-void readTemp()
+void readTemp(void)
 {
     double d;
 
@@ -39,27 +40,31 @@ void readTemp()
     }
 }
 
-void runServos()
+void runServos(void)
 {
     /* Stop */
-    OCR1A = ICR1 - 1500;
+    SERVO_L = 1500;
+    SERVO_R = 1500;
     _delay_ms(1500);
 
     /* Reverse */
-    OCR1A = ICR1 - 2000;
+    SERVO_L = 2000;
+    SERVO_R = 2000;
     _delay_ms(1500);
 
     /* Forwards */
-    OCR1A = ICR1 - 1000;
+    SERVO_L = 1000;
+    SERVO_R = 1000;
     _delay_ms(1500);
 }
 
 int main(void)
 {
-    initIO();
+    initLED();
+    initPWM();
+
     runServos();
     readTemp();
 
-
     return 0;
 }