diff --git a/esc-control.cpp b/esc-control.cpp
index e3b7c14..2fd3550 100644
--- a/esc-control.cpp
+++ b/esc-control.cpp
@@ -16,6 +16,8 @@ void esc_init()
 {
 	// Setup timer1 for PWM
 	timer1.setMode(TIMER_CH1, TIMER_PWM);
+	timer1.setMode(TIMER_CH2, TIMER_PWM);
+	timer1.setMode(TIMER_CH3, TIMER_PWM);
 	timer1.setPrescaleFactor(21);
 
 	// Set throttle to minimum
@@ -76,8 +78,6 @@ void esc_manual_control(void) {
     SerialUSB.println("Any other key zeroes command and exits");
     SerialUSB.println();
 
-    disable_usarts();
-
     uint8 input;
     float rate1 = 0;
     while (1) {
@@ -93,7 +93,7 @@ void esc_manual_control(void) {
     	}
     	else if(input == 'z')
     	{
-    		rate1 = 0;
+    		rate1 = 0.00;
 
     	}else{
     		break;
@@ -114,14 +114,5 @@ void esc_manual_control(void) {
     set_rotor_throttle(2, 0.0);
     set_rotor_throttle(3, 0.0);
 
-
-
-//    for (uint32 i = 0; i < BOARD_NR_PWM_PINS; i++) {
-//        if (boardUsesPin(i))
-//            continue;
-//        pinMode(boardPWMPins[i], OUTPUT);
-//    }
-//    init_all_timers(1);
-    enable_usarts();
 }
 
diff --git a/esc-control.h b/esc-control.h
index 1ea5e84..54f9096 100644
--- a/esc-control.h
+++ b/esc-control.h
@@ -2,6 +2,5 @@
 
 void esc_init();
 void esc_manual_control(void);
-void set_servo_angle(float angle);
 void set_rotor_throttle(int rotor, float rate);
 
diff --git a/main.cpp b/main.cpp
index 4773bf5..fd04fbc 100644
--- a/main.cpp
+++ b/main.cpp
@@ -16,7 +16,6 @@
 // -- setup() and loop() ------------------------------------------------------
 
 void setup() {
-	disable_usarts();
 
     // Set up the LED to blink
     pinMode(BOARD_LED_PIN, OUTPUT);
@@ -24,7 +23,7 @@ void setup() {
 	pinMode(ROTOR1_PIN, PWM);
 	pinMode(ROTOR2_PIN, PWM);
 	pinMode(ROTOR3_PIN, PWM);
-	// Yaw servo pin
+	// Yaw servo pin (8)
 	pinMode(YAW_SERVO_PIN, PWM);
 	// Set up PPM pin
 	pinMode(PPM_PIN, INPUT_PULLUP);
@@ -39,8 +38,8 @@ void setup() {
 	init_ppm_timer_and_dma();
 	ppm_decode_go();
 
-	// init imu rx
-	Serial3.begin(28800);
+	// imu interface init
+	imu_interface_init();
 
     // initialize usb
     SerialUSB.begin();
diff --git a/main.h b/main.h
index 20e7bea..f6b7036 100644
--- a/main.h
+++ b/main.h
@@ -11,7 +11,7 @@
  *
  */
 
-#define COPTER_DEBUG
+//#define COPTER_DEBUG
 
 /* Timer 1, Ch 1 */
 #define ROTOR1_PIN 27
@@ -19,14 +19,14 @@
 #define ROTOR2_PIN 26
 /* Timer 1, Ch 3 */
 #define ROTOR3_PIN 25
-/* Mounted on rotor 3 */
-#define YAW_SERVO_PIN 24
+/* Timer 2, Channel 4 pin */
+#define YAW_SERVO_PIN 8
 
 /* Timer 4, Ch 1 */
 #define PPM_PIN 16
 
-/* 3_RX */
-#define IMU_RX_PIN 0
+/* 3_RX  (pin 0 on Maple Mini) */
+#define IMU_RX_PIN BOARD_USART3_RX_PIN
 
 
 /* min: 1ms in tick counts */