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

DFRobot_BMX160 bmx160;
Servo s_Roll;
Servo s_Pitch;

//int curr;
//int prev;
float dR;
float dP;
//float Roll;
//float Pitch;

void setup() {
  //
  if (!bmx160.begin()  ){
    Serial.println("NO BMX found");
    while(1);
  }
  
  s_Roll.attach(2); 
  s_Pitch.attach(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){
dP = ((atan((Oaccel.y) / sqrt(pow((Oaccel.x), 2) + pow((Oaccel.z), 2))) * 4068 / 71));
dR= ((atan(-1 * (Oaccel.x) / sqrt(pow((Oaccel.y), 2) + pow((Oaccel.z), 2))) * 180 / PI));
//  dR=(atan2(Oaccel.x,Oaccel.z)*4068)/71;
//  dP=(atan2(Oaccel.y,Oaccel.z)*4068)/71;
  s_Roll.write(dR+90);
//  delay(2);
  s_Pitch.write(dP+90);
//  delay(5);
//  s_cam.write(-1*Roll);
//   s_cam.write(-1*Pitch);
//  s_cam.write(roll+90);
//    delay(5);
//    s_cam.write(-1*Oaccel.x);
//    prev=curr;
//  }