1 files changed, 13 insertions, 4 deletions
diff --git a/main.c b/main.c
index 326f4ac..3af81c5 100644
--- a/main.c
+++ b/main.c
@@ -18,8 +18,8 @@
 #define LED_DDR            DDRB
 #define LED_PORT           PORTB
 
-#define PWR_FPM            PC4
-#define PWR_SERVO          PC5
+#define PWR_SERVO          PC4
+#define PWR_FPM            PC5
 #define PWR_DDR            DDRC
 #define PWR_PORT           PORTC
 
@@ -99,7 +99,16 @@ int main(void)
 	PWR_DDR |= (1 << PWR_FPM) | (1 << PWR_SERVO);
 	pwr_fpm_servo_on();
 
-	fpm_init();
+	/* UART for FPM */
+	UBRR0H = UBRRH_VALUE;
+	UBRR0L = UBRRL_VALUE;
+#if USE_2X
+	UCSR0A |= (1 << U2X0);
+#else
+	UCSR0A &= ~(1 << U2X0);
+#endif
+	UCSR0B = (1 << TXEN0) | (1 << RXEN0);
+	UCSR0C = (1 << UCSZ01) | (1 << UCSZ00);
 
 	/* servo */
 	TCCR1A |= (1 << WGM11);
@@ -168,7 +177,7 @@ int main(void)
 		}
 
 		cmd = NONE;
-		_delay_ms(50);
+		_delay_ms(500);
 
 		pwr_fpm_servo_off();
 		set_sleep_mode(SLEEP_MODE_PWR_DOWN);