FET switches.

1 files changed, 22 insertions, 0 deletions

diff --git a/main.c b/main.c
index 9cd30d4..ddd4c18 100644
--- a/main.c
+++ b/main.c
@@ -20,6 +20,11 @@
 #define LED_DDR            DDRB
 #define LED_PORT           PORTB
 
+#define FET_FPM            PC4
+#define FET_SERVO          PC5
+#define FET_DDR            DDRC
+#define FET_PORT           PORTC
+
 #define FRONT_UNLOCK_PIN   PD2
 #define FRONT_LOCK_PIN     PD3
 #define ENROLL_PIN         PD4
@@ -91,6 +96,16 @@ static inline void flash_led(void)
 	}
 }
 
+static inline void fet_on(void)
+{
+	FET_PORT |= (1 << FET_FPM) | (1 << FET_SERVO);
+}
+
+static inline void fet_off(void)
+{
+	FET_PORT &= ~((1 << FET_FPM) | (1 << FET_SERVO));
+}
+
 int main(void)
 {
 	uint16_t id;
@@ -102,6 +117,10 @@ int main(void)
 	WDTCSR |= (1 << WDCE) | (1 << WDE);
 	WDTCSR = 0x00;
 
+	/* power on peripherals */
+	FET_DDR |= (1 << FET_FPM) | (1 << FET_SERVO);
+
+	fet_on();
 	fpm_init();
 
 	/* servo */
@@ -171,7 +190,9 @@ int main(void)
 		}
 
 		cmd = NONE;
+		_delay_ms(50);
 
+		fet_off();
 		set_sleep_mode(SLEEP_MODE_PWR_DOWN);
 		sleep_enable();
 		sleep_bod_disable();
@@ -179,6 +200,7 @@ int main(void)
 		sleep_cpu();
 		cli();
 		sleep_disable();
+		fet_on();
 	}
 
 	return 0;