commit 1226c007866d7c6048dbcf2536bb49d5355e6eab
Author: AnsonLinux <anson@ubuntu.(none)>
Date:   Mon Apr 1 13:37:56 2013 -0400

    Added motor and servo code. Both timers are set to phase-correct-PWM mode, which is yet to be tested. Also, I used 32 prescaler.

diff --git a/paintboard/code/motor.c b/paintboard/code/motor.c
new file mode 100644
index 0000000..673b68d
--- /dev/null
+++ b/paintboard/code/motor.c
@@ -0,0 +1,95 @@
+#include <avr/io.h>
+#include "motor.h"
+#include <stdlib.h>
+
+/*
+
+motor controller. Timer2 is set to phase-correct PWM mode, with 32 prescaler.
+
+AIN1: PC0
+AIN2: PC1
+PWMA: PB3 (Timer2) (OCRA)
+
+BIN1: PC2
+BIN2: PC3
+PWMB: PD3 (Timer2) (OCRB)
+
+*/
+
+int8_t motor1_speed;
+int8_t motor2_speed;
+
+void motor_init(void)
+{
+
+  // WGM2 0b001 (phase-correct PWM, 8-bit)
+  // COM2A 0b10 (clear OCR2A on compare match, set on BOTTOM)
+  // COM2B 0b10 (clear OCR2B on compare match, set on BOTTOM)
+  // CS2 0b011 (32 prescaler)
+  TCCR2A = _BV(WGM20) | _BV(COM2A1) |_BV(COM2B1);
+  TCCR2B = _BV(CS21) | _BV(CS20);
+  OCR2A = 0;
+  OCR2B = 0;
+  
+  // set output pins
+  DDRC |= _BV(PC0) | _BV(PC1) | _BV(PC2) | _BV(PC3);
+  DDRB |= _BV(PB3);
+  DDRD |= _BV(PD3);
+
+  set_motor1(0);
+  set_motor2(0);
+}
+
+void set_motor1(int8_t speed)
+/* Speed must be between -127 and 127 */
+{
+  motor1_speed = speed;
+  OCR2A = (uint8_t)abs(speed)*2;
+  if(speed>0) //go forwards
+  {
+    PORTC |= (1<<PC0);
+    PORTC = PORTC & ~(1<<PC1);
+  }  
+  if(speed<0) //go backwards
+  {
+    PORTC |= (1<<PC1);
+    PORTC = PORTC & ~(1<<PC0);
+  }
+  if(speed==0) // turn motor off
+  {
+    PORTC = PORTC & ~(1<<PC1);
+    PORTC = PORTC & ~(1<<PC0);
+  }
+}
+
+void set_motor2(int8_t speed)
+/* Speed must be between -127 and 127 */
+{
+  motor2_speed = speed;
+  OCR2B = (uint8_t)abs(speed)*2;
+  if(speed>0) //go forwards
+  {
+    PORTC |= (1<<PC2);
+    PORTC = PORTC & ~(1<<PC3);
+  }  
+  if(speed<0) //go backwards
+  {
+    PORTC |= (1<<PC3);
+    PORTC = PORTC & ~(1<<PC2);
+  }
+  if(speed==0) // turn motor off
+  {
+    PORTC = PORTC & ~(1<<PC3);
+    PORTC = PORTC & ~(1<<PC2);
+  }
+}
+
+int8_t get_motor1()
+{
+  return motor1_speed;
+}
+
+int8_t get_motor2()
+{
+  return motor2_speed;
+}
diff --git a/paintboard/code/motor.h b/paintboard/code/motor.h
new file mode 100644
index 0000000..2e318d7
--- /dev/null
+++ b/paintboard/code/motor.h
@@ -0,0 +1,10 @@
+#ifndef _MOTOR_H_
+#define _MOTOR_H_
+
+void motor_init(void);
+void set_motor1(int8_t speed);
+void set_motor2(int8_t speed);
+int8_t get_motor1();
+int8_t get_motor2();
+
+#endif
diff --git a/paintboard/code/servo.c b/paintboard/code/servo.c
new file mode 100644
index 0000000..8eedee3
--- /dev/null
+++ b/paintboard/code/servo.c
@@ -0,0 +1,60 @@
+#include <avr/io.h>
+#include "servo.h"
+#include <stdlib.h>
+
+/*
+
+servo controller. Timer0 is set to phase-correct PWM mode, with 32 prescaler.
+
+SERVO_PWM1: PD6 (Timer0)
+
+SERVO_PWM2: PD5 (Timer0)
+
+*/
+
+int8_t servo1_angle;
+int8_t servo2_angle;
+
+void servo_init()
+{
+
+  // 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);
+
+  set_servo1(0);
+  set_servo2(0);
+}
+
+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()
+{
+  return servo1_angle;
+}
+
+int8_t get_servo2()
+{
+  return servo2_angle;
+}
+
diff --git a/paintboard/code/servo.h b/paintboard/code/servo.h
new file mode 100644
index 0000000..8014ef6
--- /dev/null
+++ b/paintboard/code/servo.h
@@ -0,0 +1,10 @@
+#ifndef _SERVO_H_
+#define _SERVO_H_
+
+void servo_init();
+void set_servo1(int8_t angle);
+void set_servo2(int8_t angle);
+int8_t get_servo1();
+int8_t get_servo2();
+
+#endif
