diff --git a/P_control_Code/P_control_Code.ino b/P_control_Code/P_control_Code.ino
index dd3ff00..3dcca52 100644
--- a/P_control_Code/P_control_Code.ino
+++ b/P_control_Code/P_control_Code.ino
@@ -1,48 +1,22 @@
-// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
-// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
-// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+// NOTES
+// if the end of the axle is facing you/up:
+// rot counterclcokwise increases IMU Yaw (positively to 3.14)
+// rot clockwise decreases IMU Yaw (negatively to -3.14)
+// sending 1500+x command to servo rotates it counterclockwise
+// sending 1500-x command to servo rotates it clockwise
+// command is sent as 1500 - (rotation)*coefficient
 //
-// Changelog:
-//      2019-07-08 - Added Auto Calibration and offset generator
-//		   - and altered FIFO retrieval sequence to avoid using blocking code
-//      2016-04-18 - Eliminated a potential infinite loop
-//      2013-05-08 - added seamless Fastwire support
-//                 - added note about gyro calibration
-//      2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
-//      2012-06-20 - improved FIFO overflow handling and simplified read process
-//      2012-06-19 - completely rearranged DMP initialization code and simplification
-//      2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly
-//      2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING
-//      2012-06-05 - add gravity-compensated initial reference frame acceleration output
-//                 - add 3D math helper file to DMP6 example sketch
-//                 - add Euler output and Yaw/Pitch/Roll output formats
-//      2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)
-//      2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250
-//      2012-05-30 - basic DMP initialization working
+//
+//
+//
+//
+//
+//Upd. 10/29 2:25 PM
+
 
-/* ============================================
-I2Cdev device library code is placed under the MIT license
-Copyright (c) 2012 Jeff Rowberg
 
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
 
-The above copyright notice and this permission notice shall be included in
-all copies or substantial portions of the Software.
 
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-THE SOFTWARE.
-===============================================
-*/
 
 // I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
 // for both classes must be in the include path of your project
@@ -89,7 +63,7 @@ MPU6050 mpu(0x69); // <-- use for AD0 high
 
 double tic = millis();
 double goal = 0.0;
-double kp = 1;
+double kp = 750;
 double ki = 1;
 double kd = 1;
 
@@ -264,7 +238,7 @@ myservo.attach(3);
         Serial.println(F(")"));
     }
 
-    // configure LED for output
+    // configure LED for outputj
     pinMode(LED_PIN, OUTPUT);
 }
 
@@ -300,9 +274,9 @@ void loop() {
             properr = goal-ypr[0];
             //interr += properr * (toc-tic);
             Serial.println(properr*kp);
-
-            int servoVal = map(properr * kp, -3.15 * kp, 3.15 * kp, 1000, 2000);
-            myservo.writeMicroseconds(servoVal);
+            int toRot = properr*kp;
+            int Rot = max(min(1500-toRot,2000),1000);
+            myservo.writeMicroseconds(Rot);