commit d9b50611a8889856a5ab31a3dcdc6856e8f16c8b
Author: Tom Mullins <tmullins@andrew.cmu.edu>
Date:   Thu Apr 18 18:28:43 2013 -0400

    Fixed servo code to output correct signal

diff --git a/paintboard/code/main.c b/paintboard/code/main.c
index 55164b4..d58b598 100644
--- a/paintboard/code/main.c
+++ b/paintboard/code/main.c
@@ -4,6 +4,7 @@
 #include "sol.h"
 #include <avr/io.h>
 #include <avr/interrupt.h>
+#include <util/delay.h>
 
 #define TRACKING_ID 0x43
 #define SERIAL_NUMBER 0x12
@@ -26,7 +27,7 @@
 
 #define PAINT_DATA_LEN         13
 
-#define METAL_DETECT           PD4
+#define METAL_DETECT           PD5
 
 uint8_t internal_index = 0;
 uint8_t internal_data[PAINT_DATA_LEN] = {
@@ -105,7 +106,14 @@ int main()
   while (1)
   {
     internal_data[PAINT_INPUT_1] = !(!(_BV(METAL_DETECT) & PIND));
+    if (internal_data[PAINT_INPUT_1]) {
+      set_servo1(-128);
+    } else {
+      set_servo1(127);
+    }
+    servo_pulse();
     /* TODO geiger counter */
+    _delay_ms(20);
   }
   return 0;
 }
diff --git a/paintboard/code/servo.c b/paintboard/code/servo.c
index 8eedee3..9236657 100644
--- a/paintboard/code/servo.c
+++ b/paintboard/code/servo.c
@@ -1,10 +1,15 @@
 #include <avr/io.h>
-#include "servo.h"
+#include <avr/interrupt.h>
 #include <stdlib.h>
+#include "servo.h"
 
 /*
 
-servo controller. Timer0 is set to phase-correct PWM mode, with 32 prescaler.
+Servo controller. Timer0 is set to fast PWM mode, with 64 prescaler. Outputs
+are cleared on BOTTOM and set on compare match, and the compare values are set
+to 255 minus the regular value. This way the timer can be stopped when it
+overflows and the output will end up off, but will have been on for the correct
+amount of time.
 
 SERVO_PWM1: PD6 (Timer0)
 
@@ -12,40 +17,64 @@ SERVO_PWM2: PD5 (Timer0)
 
 */
 
+#define PRESCALER 64
+#define MS_TO_TICKS(x) ((uint16_t)((x) * (F_CPU / 1000 / PRESCALER)))
+
+#define MIN_TICKS MS_TO_TICKS(1)
+#define MAX_TICKS MS_TO_TICKS(2)
+
+static uint8_t angle_to_ticks(int8_t angle)
+{
+  return ((int16_t)angle + 128) * (MAX_TICKS - MIN_TICKS) / 256 + MIN_TICKS;
+}
+
 int8_t servo1_angle;
 int8_t servo2_angle;
 
 void servo_init()
 {
+  // WGM0 0b011 (fast pwm)
+  // COM0A 0b11 (set OCR0A on compare match, clear on BOTTOM)
+  // COM0B 0b11 (set OCR0B on compare match, clear on BOTTOM)
+  // B is temporarily disabled grrrrr
+  //TCCR0A = _BV(WGM01) | _BV(WGM00) | _BV(COM0A1) | _BV(COM0A0) | _BV(COM0B1)
+  //    | _BV(COM0B0); TODO temporary
+  TCCR0A = _BV(WGM01) | _BV(WGM00) | _BV(COM0A1) | _BV(COM0A0);
+  TCCR0B = 0;
+
+  // enable overflow interrupt
+  TIMSK0 = _BV(TOIE0);
 
-  // WGM0 0b001 (phase-correct PWM, 8-bit)
-  // COM0A 0b10 (clear OCR0A on compare match, set on BOTTOM)
-  // COM0B 0b10 (clear OCR0B on compare match, set on BOTTOM)
-  // CS0 0b011 (32 prescaler)
-  TCCR0A = _BV(WGM00) | _BV(COM0A1) |_BV(COM0B1);
-  TCCR0B = _BV(CS01) | _BV(CS00);
-  OCR0A = 0;
-  OCR0B = 0;
-  
   // set output pins
-  DDRD |= _BV(PD5) | _BV(PD6);
+  //DDRD |= _BV(PD5) | _BV(PD6); TODO temporary
+  DDRD |= _BV(PD6);
 
   set_servo1(0);
   set_servo2(0);
 }
 
+void servo_pulse()
+{
+  // set limits, turn on outputs, and start timer
+
+  OCR0A = 255 - angle_to_ticks(servo1_angle);
+  OCR0B = 255 - angle_to_ticks(servo2_angle);
+  TCNT0 = 0;
+
+  // CS0 0b011 (64 prescaler)
+  TCCR0B = _BV(CS01) | _BV(CS00);
+}
+
 void set_servo1(int8_t angle)
 /* angle must be between -128 and 127 */
 {
   servo1_angle = angle;
-  OCR0A = (uint8_t)(angle + 128);
 }
 
 void set_servo2(int8_t angle)
 /* angle must be between -128 and 127 */
 {
   servo2_angle = angle;
-  OCR0B = (uint8_t)(angle + 128);
 }
 
 int8_t get_servo1()
@@ -58,3 +87,8 @@ int8_t get_servo2()
   return servo2_angle;
 }
 
+ISR(TIMER0_OVF_vect)
+{
+  // stop timer
+  TCCR0B = 0;
+}
diff --git a/paintboard/code/servo.h b/paintboard/code/servo.h
index 8014ef6..8a4a5f1 100644
--- a/paintboard/code/servo.h
+++ b/paintboard/code/servo.h
@@ -2,6 +2,7 @@
 #define _SERVO_H_
 
 void servo_init();
+void servo_pulse(); // should be called ~every 20ms
 void set_servo1(int8_t angle);
 void set_servo2(int8_t angle);
 int8_t get_servo1();
