   
   devStatus = mpu.dmpInitialize();
 
+  // accel offsets
+  mpu.setXAccelOffset(-4265);
+  mpu.setYAccelOffset(-283);
+  mpu.setZAccelOffset(1675); // 1688 factory default for my test chip
   // supply your own gyro offsets here, scaled for min sensitivity
-  mpu.setXGyroOffset(220);
-  mpu.setYGyroOffset(76);
-  mpu.setZGyroOffset(-85);
-  mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
+  mpu.setXGyroOffset(-51);
+  mpu.setYGyroOffset(40);
+  mpu.setZGyroOffset(27);
 
   // make sure it worked (returns 0 if so)
   if (devStatus == 0) {

#define LOW_START 3230
#define HIGH_START 4370

#define LEFT_MOTOR_1A 42
#define LEFT_MOTOR_1B 3

#define INTERRUPT_PIN 2

#define setpoint 0
#define kP 0.05



        switch (axis) {
          case 'x':
            return (euler[2] * 180/M_PI) - gyroResetValX;
            break;
          case 'y':
            return (euler[1] * 180/M_PI) - gyroResetValY;
            break;
          case 'z':
            return (euler[0] * 180/M_PI) - gyroResetValZ;
            break;
        }
    }
}

void resetGyro(char axis) {
  switch (axis) {
    case 'x':
      gyroResetValX += getAngle('x');
      break;
    case 'y':
      gyroResetValY += getAngle('y');
      break;
    case 'z':
      gyroResetValZ += getAngle('z');
      break;
  }
}

