From a843375c7e1856305aa6c3103d1ed5b90eb0a1e1 Mon Sep 17 00:00:00 2001
From: Rihards Skuja <rssk@protonmail.com>
Date: Mon, 22 Jan 2018 13:01:37 +0200
Subject: [PATCH] Fix Fast PWM output for servos

---
 robot/include/servos.h | 31 +++++++++++++++++++------------
 robot/src/main.c       |  6 +++---
 robot/src/servos.c     | 18 ++++++++++--------
 3 files changed, 32 insertions(+), 23 deletions(-)

diff --git a/robot/include/servos.h b/robot/include/servos.h
index de3aad4..4ede364 100644
--- a/robot/include/servos.h
+++ b/robot/include/servos.h
@@ -1,22 +1,29 @@
 #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       OCR0A
+#define SERVO_R_REG       OCR0B
+#define SERVO_L_BIT       PD6
+#define SERVO_R_BIT       PD5
 
-#define SERVO_MIN       930 // (us). Go backwards
-#define SERVO_MAX       1930 // (us). Go forwards
-#define SERVO_MID       (SERVO_MIN + SERVO_MAX) / 2 // (us). Stop
+// Not just STOP +- 500us because of rounding errors (e.g. 19.9 will become
+// 19 and not 20 because of integer division)
+#define SERVO_MID         1381UL
+#define SERVO_MAX         1883UL
+#define SERVO_MIN         880UL
 
-#define PRESCALER       1024L
-#define PWM_CLOCK       F_CPU / (256 * PRESCALER)
-#define PWM_PERIOD_US   1000000 / PWM_CLOCK
+#define SERVO_L_STOP      SERVO_MID
+#define SERVO_L_FORW      SERVO_MAX
+#define SERVO_L_BACKW     SERVO_MIN
 
-#define US2TIMER0(us)   (255 * (uint32_t)us) / PWM_PERIOD_US
+#define SERVO_R_STOP      SERVO_MID
+#define SERVO_R_FORW      SERVO_MIN
+#define SERVO_R_BACKW     SERVO_MAX
 
+#define PRESCALER         1024UL
+#define PWM_PERIOD_MS     (uint16_t)((256000UL * PRESCALER) / F_CPU)
+#define US2TIMER0(us)     (uint16_t)((51U * us) / (200U * PWM_PERIOD_MS))
 
 void init_servos(void);
 void run_servos(void);
diff --git a/robot/src/main.c b/robot/src/main.c
index e265e44..b706bf0 100644
--- a/robot/src/main.c
+++ b/robot/src/main.c
@@ -26,7 +26,7 @@ int main(void)
     tx_raw = malloc(sizeof(data_packet_t));
 
     init_leds();
-    /* init_servos(); */
+    init_servos();
     init_sonar();
     nrf24_init();
 
@@ -40,8 +40,8 @@ int main(void)
     /* SET(LED_PORT, LED_GREEN_BIT); */
 
     sei();
-    /* run_servos(); */
-    /* read_temp(); */
+
+    run_servos();
 
     while (1) {
         uint16_t distance_int_cm = read_sonar();
diff --git a/robot/src/servos.c b/robot/src/servos.c
index df4bd5a..0f6bd11 100644
--- a/robot/src/servos.c
+++ b/robot/src/servos.c
@@ -5,11 +5,10 @@
 
 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
+    // Clear OC0A on Compare Match, set OC0A at BOTTOM (non-inverting mode)
+    SET(TCCR0A, COM0A1);
+    // Clear OC0B on Compare Match, set OC0B at BOTTOM, (non-inverting mode)
+    SET(TCCR0A, COM0B1);
 
     // Fast PWM, 0xFF TOP
     SET(TCCR0A, WGM00);
@@ -18,11 +17,14 @@ void init_servos(void)
     // Prescaler = 1024
     SET(TCCR0B, CS00);
     SET(TCCR0B, CS02);
+
+    // Set sevo pins as outputs
+    SET(SERVO_DDR, SERVO_L_BIT);
+    SET(SERVO_DDR, SERVO_R_BIT);
 }
 
 void run_servos(void)
 {
-    SERVO_L_REG = US2TIMER0(SERVO_MAX);
-    SERVO_R_REG = US2TIMER0(SERVO_MID);
-    _delay_ms(10000); // 10s
+    SERVO_L_REG = US2TIMER0(SERVO_L_FORW);
+    SERVO_R_REG = US2TIMER0(SERVO_R_FORW);
 }