 #define RC_5 8
 #define RC_PWR A0
 
-// PID configuration
-#define pKp 2
-#define pKi 5
-#define pKd 1
-
-#define rKp 0.1
-#define rKi 0.05
-#define rKd 0.25
-
-#define yKp 0.1
-#define yKi 0.05
-#define yKd 0.25
-
-/*
-  Arducopter v1.0
-  
-  RC Channels: 
-    
-    Throttle - CH3
-    Pitch    - CH2
-    Roll     - CH1
-    Yaw      - CH4
-    Aux      - CH5
-
-
-*/
-
-// MPU SETUP //
-
-MPU6050 mpu; // mpu interface object
-
-bool dmpReady = false; //flag
-uint8_t mpuIntStatus;  //mpu statusbyte
-uint8_t devStatus; //device status byte    
-uint16_t packetSize; //estimated packet size  
-uint16_t fifoCount; //fifo buffer size   
-uint8_t fifoBuffer[64]; //fifo buffer 
-
-Quaternion q; //quaternion for mpu output
-VectorFloat gravity; //gravity vector for ypr
-float ypr[3] = {0.0f,0.0f,0.0f}; // yaw pitch roll values
-
-volatile bool mpuInterrupt = false; //interrupt flag
-
-// RC/CONTROL SETUP
-
-float ch1, ch2, ch3, ch4, ch5; //RC channel inputs
-float ch3_smooth = 1000;
-int va, vb, vc, vd; //velocities
-int v_ac, v_bd; // velocity of axes
+
+/* ESC configuration
+ *
+ */
+
+#define ESC_MIN 22
+#define ESC_MAX 115
+#define ESC_TAKEOFF_OFFSET 30
+#define ESC_ARM_DELAY 5000
+
+/* RC configuration
+ * 
+ */
+
+#define RC_HIGH 1000
+#define RC_LOW 2000
+
+/*  PID configuration
+ *  
+ */
+
+#define PITCH_P_VAL 2
+#define PITCH_I_VAL 5
+#define PITCH_D_VAL 1
+
+#define ROLL_P_VAL 2
+#define ROLL_I_VAL 5
+#define ROLL_D_VAL 1
+
+#define YAW_P_VAL 2
+#define YAW_I_VAL 5
+#define YAW_D_VAL 1
+
+
+/* Flight parameters
+ *
+ */
+
+#define PITCH_MIN -45
+#define PITCH_MAX 45
+#define ROLL_MIN -45
+#define ROLL_MAX 45
+#define YAW_MIN -180
+#define YAW_MAX 180
+#define PID_PITCH_INFLUENCE 15
+#define PID_ROLL_INFLUENCE 15
+#define PID_YAW_INFLUENCE 15
+
+
+/*  MPU variables
+ *
+ */
+
+MPU6050 mpu;                           // mpu interface object
+
+
+uint8_t mpuIntStatus;                  // mpu statusbyte
+uint8_t devStatus;                     // device status    
+uint16_t packetSize;                   // estimated packet size  
+uint16_t fifoCount;                    // fifo buffer size   
+uint8_t fifoBuffer[64];                // fifo buffer 
+
+Quaternion q;                          // quaternion for mpu output
+VectorFloat gravity;                   // gravity vector for ypr
+float ypr[3] = {0.0f,0.0f,0.0f};       // yaw pitch roll values
+
+volatile bool mpuInterrupt = false;    //interrupt flag
+
+
+/*  RC variables
+ *
+ */
+
+float ch1, ch2, ch3, ch4, ch5;         // RC channel inputs
+float ch3_smooth = 1000;               // may be unnecessary with new input method
 
 unsigned long rcLastChange1 = millis();
 unsigned long rcLastChange2 = millis();

    mpu.setDMPEnabled(true);
    attachInterrupt(0, dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();
    dmpReady = true;
    packetSize = mpu.dmpGetFIFOPacketSize();
    
  }


}

void initRegulators(){

  pitchReg.SetMode(AUTOMATIC);
  pitchReg.SetOutputLimits(-PID_PITCH_INFLUENCE, PID_PITCH_INFLUENCE);
  
  rollReg.SetMode(AUTOMATIC);
  rollReg.SetOutputLimits(-PID_ROLL_INFLUENCE, PID_ROLL_INFLUENCE);
  
  yawReg.SetMode(AUTOMATIC);
  yawReg.SetOutputLimits(-PID_YAW_INFLUENCE, PID_YAW_INFLUENCE);

}

void rcInterrupt1(){
   ch1 = millis() - rcLastChange1;
   rcLastChange1 = millis(); 
