void loop()
{
int8_t x;
int8_t y;
int8_t z;
float ax,ay,az;
accelemeter.getXYZ(&x,&y,&z);
Serial.print("x = ");
Serial.println(x);
Serial.print("y = ");
Serial.println(y);
Serial.print("z = ");
Serial.println(z);
accelemeter.getAcceleration(&ax,&ay,&az);
Serial.println("accleration of X/Y/Z: ");
Serial.print(ax);
Serial.println(" g");
Serial.print(ay);
Serial.println(" g");
Serial.print(az);
Serial.println(" g");
Serial.println("*************");
delay(500);
}
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 loop() {
unsigned char data[8];
if (mySerial.available() > 0) {
if (readFrame(&mySerial, data) == FRAME_COMPLETE)
controller.updateFromDataArray(data);
}
acc.read();
gyro.read();
alt.start();
quad.computePIDs();
quad.setESCs();
}
void setup()
{
accelemeter.init();
Serial.begin(9600);
}
