diff options
| -rw-r--r-- | main.c | 35 |
1 files changed, 20 insertions, 15 deletions
@@ -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; |