   
   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) {

Motor strafeMotor(10, 11);
Motor fan_motor(12, 13);

ColorSensor colour_sensor(44, 46, 48, 50, 52);

Ultrasonic frontUltrasonic(28, 29, true);
Ultrasonic leftUltrasonic(26, 27, true);

      packetSize = mpu.dmpGetFIFOPacketSize();
  }

  resetGyro('z');

  FreqCount.begin(1000);
}

bool firstStart = false;
bool foundFlame = false;

void loop() {
  // put your main code here, to run repeatedly:
  if (FreqCount.available()) {
    count = FreqCount.read();
	  //Serial.println(count);
  } else {
    count = 0;
  }
//  Serial.println(getAngle('z'));
  if ((count > 3230 && count < 4370) || digitalRead(startSwitch)) {
    digitalWrite(SOUND_LED, HIGH);
    firstStart = true;
    robotOn = !robotOn;
    resetGyro('z');
  }
  
//  Serial.println(getAngle('z'));
  if(robotOn){
    if (firstStart) {
      FreqCount.end();
      ri = colour_sensor.getColor('r');
    }
	
  	while(!foundFlame && digitalRead(flame_sensor)) {
  		// naviguessMaze(0.3);
  		// followWall(1.0);
       ultrasonicCheck();
      // drivetrainCheck();
      
//      if (!pidEnabled) {
//        pidEnabled = true;
//        resetGyro('z');
//      }
//      hDrive(0.5, getPidOutput(), 0.0);   // Correctional driving
//      Serial.println(getPidOutput());

  	}
	
  	foundFlame = true;
  	digitalWrite(FLAME_LED, foundFlame);
  	
  	fan_motor.set(1.0);
  	
  	hDrive(0.0, 0.0, 0.0);
  	delay(100);
	
  	while (frontUltrasonic.getDistance() > 0.5) {
  		hDrive(0.3, 0.0, 0.0);
  		extingusher_servo.write(110);
  		delay(600);
  		extingusher_servo.write(70);
  		delay(600);
  		hDrive(0.0, 0.0, 0.0);
  		delay(200);
  		digitalWrite(FLAME_LED, !digitalRead(flame_sensor));
  	}

	  hDrive(0.0, 0.0, 0.0);
  }
  else{
    digitalWrite(SOUND_LED, LOW);
    firstStart = false;
    hDrive(0.0, 0.0, 0.0);
  }

//  delay(100);
}

void hDrive(double move, double rotate, double strafe) {
