More fine-grained pwr controls.

1 files changed, 20 insertions, 15 deletions

diff --git a/main.c b/main.c
index 9c43e20..5925c02 100644
--- a/main.c
+++ b/main.c
@@ -53,16 +53,30 @@ enum CTRL {
 
 static volatile enum CTRL cmd = NONE;
 
+static inline void pwron(uint8_t port)
+{
+	PWR_PORT |= (1 << port);
+}
+
+static inline void pwroff(uint8_t port)
+{
+	PWR_PORT &= ~(1 << port);
+}
+
 static inline void lock(void)
 {
+	pwron(PWR_SERVO);
 	OCR1A = PWM_MID;
 	_delay_ms(500);
+	pwroff(PWR_SERVO);
 }
 
 static inline void unlock(void)
 {
+	pwron(PWR_SERVO);
 	OCR1A = PWM_MAX;
 	_delay_ms(500);
+	pwroff(PWR_SERVO);
 }
 
 static inline void flash_led(void)
@@ -75,16 +89,6 @@ static inline void flash_led(void)
 	}
 }
 
-static inline void pwr_fpm_servo_on(void)
-{
-	PWR_PORT |= (1 << PWR_FPM) | (1 << PWR_SERVO);
-}
-
-static inline void pwr_fpm_servo_off(void)
-{
-	PWR_PORT &= ~((1 << PWR_FPM) | (1 << PWR_SERVO));
-}
-
 int main(void)
 {
 	uint16_t id;
@@ -97,10 +101,9 @@ int main(void)
 	WDTCSR = 0x00;
 
 	uart_init();
-
-	/* power on the FPM and the servo */
 	PWR_DDR |= (1 << PWR_FPM) | (1 << PWR_SERVO);
-	pwr_fpm_servo_on();
+	pwron(PWR_FPM);
+	fpm_init();
 
 	/* servo */
 	TCCR1A |= (1 << WGM11);
@@ -171,15 +174,17 @@ int main(void)
 		cmd = NONE;
 		_delay_ms(500);
 
-		pwr_fpm_servo_off();
+		pwroff(PWR_FPM);
 		set_sleep_mode(SLEEP_MODE_PWR_DOWN);
 		sleep_enable();
 		sleep_bod_disable();
 		sei();
 		sleep_cpu();
+
 		cli();
 		sleep_disable();
-		pwr_fpm_servo_on();
+		pwron(PWR_FPM);
+		fpm_init();
 	}
 
 	return 0;