Weird Bug (IC2, PWM)

Projects Programming
1

So I ant to control some esc's with my arduino using pwm signals. Everything works fine until I do some stuff with IC2 to read from a sensor, then the esc'sjust go crazy, and turn on and off. So I would really appreciate if someone could help me solve this issue.

Here's the code:

Update loop:

#include "IMU.h"
#include "PID.h"
#include "Motor.h"
#include "Receiver.h"
#include "LowPass.h"

PID pid_pitch(1.8, 0, 0);
PID pid_roll(1.8, 0, 0);
PID pid_yaw(1, 0, 0);


void setup() {
	Serial.begin(9600);

	//initReceiver();
	//delay(2000);

	initIMU();
	delay(2000);

	initMotors();
	delay(2000);

	armMotors();
	delay(4000);
}

long hz330 = 0;
long hz200 = 0;
long hz50 = 0;

LowPass rx_pitch;
LowPass rx_roll;
LowPass rx_yaw;
LowPass rx_throttle;


bool armed = false;

void loop() {

	if(micros() - hz330 > 3000) {
		hz330 = micros();
		updateGyro();
	}

	if(micros() - hz200 > 5000) {
		hz200 = micros();
		updateIMU();

		int throttle_offset = rxChannels[AUX_1_CHANNEL];

		rx_pitch.updateFilter(map(rxChannels[PITCH_CHANNEL], 1100, 1900, 30, -30));
		rx_roll.updateFilter(map(rxChannels[ROLL_CHANNEL], 1100, 1900, 30, -30));
		rx_yaw.updateFilter(map(rxChannels[PITCH_CHANNEL], 1100, 1900, -30, 30));
		rx_throttle.updateFilter(map(rxChannels[THROTTLE_CHANNEL], 1100, 1900, -15, 15));

		float pitch = pid_pitch.updatePosition(-orientation[0], gyro[1].value / 50, rx_pitch.value);
		float roll = pid_roll.updatePosition(-orientation[1], -gyro[0].value / 50, rx_roll.value);
		float yaw = pid_yaw.updateVelocity(gyro[2].value / 50, rx_yaw.value);


		/*if(armed) {
			writeMotors(throttle_offset + rx_throttle.value + pitch + roll + yaw,
						throttle_offset + rx_throttle.value + pitch - roll - yaw,
						throttle_offset + rx_throttle.value - pitch - roll + yaw,
						throttle_offset + rx_throttle.value - pitch + roll - yaw);
		}

		if(!armed && rxChannels[AUX_0_CHANNEL] > 1500) {
			armed = true;
			armMotors();
			delay(4000);
		} else if(armed && rxChannels[AUX_0_CHANNEL] < 1500) {
			armed = false;
			writeMotors(0, 0, 0, 0);
		}*/
	}
	
	if(micros() - hz50 > 20000) {
		hz50 = micros();
		updateAcc();
	}
}

Sensor reading:

void updateGyro() {
	readLen(BNO055_GYRO_DATA_X_LSB_ADDR, buffer, 6);

	gyro[0].updateFilter((float)(((int)buffer[0]) | ((int)buffer[1] << 8))); 
	gyro[1].updateFilter((float)(((int)buffer[2]) | ((int)buffer[3] << 8))); 
	gyro[2].updateFilter((float)(((int)buffer[4]) | ((int)buffer[5] << 8)));
}

void updateAcc() {
	readLen(BNO055_ACCEL_DATA_X_LSB_ADDR, buffer, 6);

	float aX = (float)(((int)buffer[0]) | ((int)buffer[1] << 8)) / 16;
	float aY = (float)(((int)buffer[2]) | ((int)buffer[3] << 8)) / 16;
	float aZ = (float)(((int)buffer[4]) | ((int)buffer[5] << 8)) / 16;

	acc[0].updateFilter(atan2(aX, aZ) * RAD_TO_DEG);
	acc[1].updateFilter(atan2(aY, aZ) * RAD_TO_DEG);
}

void readLen(adafruit_bno055_reg_t reg, byte* buffer, int len) {
	Wire.beginTransmission(BNO055_ADDRESS_A);
    Wire.write((byte)reg);
  	Wire.endTransmission();
  	Wire.requestFrom(BNO055_ADDRESS_A, len);

	for (byte i = 0; i < len; i++)
	{
	      buffer[i] = Wire.read();
	}
}

Motor stuff:

void initMotors() {
	motor1.attach(MOTOR_PIN_1);
	motor2.attach(MOTOR_PIN_2);
	motor3.attach(MOTOR_PIN_3);
	motor4.attach(MOTOR_PIN_4);
}

void writeMotors(int motor1_val, int motor2_val, 
	int motor3_val, int motor4_val) {
	motor1.write(motor1_val + MIN_THROTTLE);
	motor2.write(motor2_val + MIN_THROTTLE);
	motor3.write(motor3_val + MIN_THROTTLE);
	motor4.write(motor4_val + MIN_THROTTLE);
}

void armMotors() {
	motor1.write(1050);
	motor2.write(1050);
	motor3.write(1050);
	motor4.write(1050);
}