// put your setup code here, to run once:

pinMode(STATUS_FLASH_PIN, OUTPUT);

pinMode(OUT_TR, OUTPUT);

pinMode(OUT_BR, OUTPUT);

pinMode(OUT_TL, OUTPUT);

pinMode(OUT_BL, OUTPUT);

pinMode(ZERO_BUTTON, INPUT);

digitalWrite(STATUS_FLASH_PIN, HIGH);

SPI.begin();

Serial.begin(9600);

while (!accel.begin(ADXL355_RANGE_2G, ADXL355_FILTER_LPF_LOW_FREQ)) {

digitalWrite(STATUS_FLASH_PIN, LOW);

delay(100);

digitalWrite(STATUS_FLASH_PIN, HIGH);

delay(100);

Serial.println("ADXL355 Failed to initialize");

}

Serial.println("ADXL355 Initialized");

delay(200);

//Allows a preconfigured offset (useful for YAW)

//All rotations are in Roll-Pitch-Yaw (Left to right).

accelModel.setBaseFrameAnglesRadians(Eigen::Vector3d(0,0,0));

// put your main code here, to run repeatedly:

accel.takeSample();

ADXL355Measurement measure = accel.getSample();

ewmaFilter.addData(measure);

measure = ewmaFilter.getAverage();

Eigen::Vector2d angles = accelModel.calculate(Eigen::Vector3d(measure.x, measure.y, measure.z));

Serial.print(angles[0] * 180.0 / PI);

Serial.print("\t");

Serial.print(angles[1] * 180.0 / PI);

Serial.println();

cornerAlgo.update(angles[0], angles[1]);

digitalWrite(OUT_TR, cornerAlgo.getCorner(0));

digitalWrite(OUT_TL, cornerAlgo.getCorner(1));

digitalWrite(OUT_BL, cornerAlgo.getCorner(2));

digitalWrite(OUT_BR, cornerAlgo.getCorner(3));

if (!digitalRead(ZERO_BUTTON)) {

//accelModel.setMeasurementAsZero(Eigen::Vector3d(measure.x, measure.y, measure.z));

}

delay(50);