void setup() {
debug("Starting setup");
Serial.begin(9600);
mySerial.begin(9600);
// Initialize classes
escs.x = &esc_x;
escs.nx = &esc_nx;
escs.y = &esc_y;
escs.ny = &esc_ny;
quad.escs = &escs;
quad.controller = &controller;
quad.gyro = &gyro;
quad.acc = &acc;
quad.alt = &alt;
Wire.begin();
acc.init();
gyro.init();
attachInterrupt(PING_INT, pingInterrupt, FALLING);
pinMode(ESC_X_PIN, OUTPUT);
pinMode(ESC_NX_PIN, OUTPUT);
pinMode(ESC_Y_PIN, OUTPUT);
pinMode(ESC_NY_PIN, OUTPUT);
// Set all motor speeds to ESC min
escs.write(OUTPUT_MIN);
debug("Entering main loop");
}
void ITG3200_init(){
#ifdef DEBUG_ITG3200_POLLER
DEBUG_1("Starting");
#endif
Wire.begin();
delay(1000);
itg.init(ITG3200_ADDR_AD0_HIGH);
#ifdef DEBUG_ITG3200_POLLER
DEBUG_1("Finished");
#endif
}
void Cfilterbegin(){
//Turning on the accelerometer
Accel.init(ADXL345_ADDR_ALT_LOW);
Accel.set_bw(ADXL345_BW_12);
gyro.reset();
// Use ITG3200_ADDR_AD0_HIGH or ITG3200_ADDR_AD0_LOW as the ITG3200 address
// depending on how AD0 is connected on your breakout board, check its schematics for details
gyro.init(ITG3200_ADDR_AD0_LOW);
Serial.print("zeroCalibrating...");
gyro.zeroCalibrate(2500,2);
Serial.println("done.");
alpha = float(TIME_CONSTANT) / (float(TIME_CONSTANT) + float(SAMPLE_RATE)); // calculate the scaling coefficent
Serial.print("Scaling Coefficent: ");
Serial.println(alpha);
delay(100);
}