Servo control.

1 files changed, 33 insertions, 1 deletions

diff --git a/main.c b/main.c
index abc5b1b..7aea82a 100644
--- a/main.c
+++ b/main.c
@@ -25,9 +25,16 @@
 #define BACK_UNLOCK_INT    PCINT22
 #define BTN_INT_VEC        PCINT2_vect
 
+#define SERVO_PIN          PB1
+
+#define PWM_MIN            500
+#define PWM_MID            1500
+#define PWM_MAX            2500
+#define PWM_TOP            20000
+
 int main(void)
 {
-	/* input ports */
+	/* init input ports */
 	INPUT_DDR &= ~((1 << BACK_LOCK_PIN) | (1 << BACK_UNLOCK_PIN) | 
 	               (1 << FRONT_LOCK_PIN) | (1 << FRONT_UNLOCK_PIN) | 
 	               (1 << ENROLL_PIN));
@@ -43,6 +50,13 @@ int main(void)
 	PCMSK2 |= ((1 << FRONT_LOCK_INT) | (1 << ENROLL_INT) | 
 	           (1 << BACK_LOCK_INT) | (1 << BACK_UNLOCK_INT));
 
+	/* init servo */
+	ICR1 = PWM_TOP;
+	TCCR1A |= (1 << WGM11) | (1 << COM1A1);
+	TCCR1B |= (1 << WGM13) | (1 << CS11);
+
+	DDRB |= (1 << SERVO_PIN);
+
 	fpm_init();
 	sei();
 
@@ -52,6 +66,20 @@ int main(void)
 	return 0;
 }
 
+static inline void lock(void)
+{
+	OCR1A = PWM_MID;
+	_delay_ms(100);
+	OCR1A = PWM_TOP;
+}
+
+static inline void unlock(void)
+{
+	OCR1A = PWM_MAX - 50;
+	_delay_ms(100);
+	OCR1A = PWM_TOP;
+}
+
 static inline int is_pressed(uint8_t btn)
 {
 	if (!((PIND >> btn) & 0x01)) {
@@ -66,6 +94,7 @@ ISR(FPM_INT_VEC)
 	cli();
 
 	if (fpm_match()) {
+		unlock();
 		fpm_led(BREATHE, BLUE, 1);
 	} else {
 		fpm_led(BREATHE, RED, 1);
@@ -81,9 +110,12 @@ ISR(BTN_INT_VEC)
 	cli();
 
 	if (is_pressed(FRONT_LOCK_PIN)) {
+		lock();
 		fpm_led(FLASH, RED, 1);
 	} else if (is_pressed(BACK_LOCK_PIN)) {
+		lock();
 	} else if (is_pressed(BACK_UNLOCK_PIN)) {
+		unlock();
 	} else if (is_pressed(ENROLL_PIN)) {
 		id = fpm_match();
 		if (id == 1 || id == 2) {