#include <DFRobot_BMX160.h>
#include <Servo.h>
#include <SoftwareSerial.h>

DFRobot_BMX160 bmx160;
Servo s_cam;

//int curr;
//int prev;
//float d;
float Roll;
float Pitch;

void setup() {
  //
  if (!bmx160.begin()  ){
    Serial.println("NO BMX found");
    while(1);
  }
  
  s_cam.attach(28); //actual servo will be at 24

}

void loop() {
  sBmx160SensorData_t Omagn, Ogyro, Oaccel;
  bmx160.getAllData(NULL, NULL, &Oaccel);

//  curr=map(Oaccel.x,-17000,17000,179,0);
//  curr=map(Oaccel.x,-180,180,180,-180);
//  if (curr!=prev){
  //roll = atan2(y_Buff , z_Buff) * 57.3;
  //pitch = atan2((- x_Buff) , sqrt(y_Buff * y_Buff + z_Buff * z_Buff)) * 57.3;
  //d=(atan2(Oaccel.x,Oaccel.z)*4068)/71;
  //  Roll = ((atan((Oaccel.y) / sqrt(pow((Oaccel.x), 2) + pow((Oaccel.z), 2))) * 180 / PI));
  //  Pitch = ((atan(-1 * (Oaccel.x) / sqrt(pow((Oaccel.y), 2) + pow((Oaccel.z), 2))) * 180 / PI));
  s_cam.write(-1*Roll);
  // s_cam.write(roll+90);
//    delay(5);
//    s_cam.write(-1*Oaccel.x);
//    prev=curr;
//  }


}