/*!
  * imu_show.ino
  *
  * Download this demo to show attitude on [imu_show](https://github.com/DFRobot/DFRobot_IMU_Show)
  * Attitude will show on imu_show
  *
  * Product: https://www.dfrobot.com.cn/goods-1860.html
  * Copyright   [DFRobot](https://www.dfrobot.com), 2016
  * Copyright   GNU Lesser General Public License
  *
  * version  V1.0
  * date  07/03/2019
  */

#include "DFRobot_BNO055.h"
#include "Wire.h"

typedef DFRobot_BNO055_IIC    BNO;    // ******** use abbreviations instead of full names ********

BNO   bno(&Wire, 0x28);    // input TwoWire interface and IIC address
float  start_st = 0;
float  head_st = 0;
float  temp_st = 0;
float  roll_st = 0;
float  pitch_st = 0;
// show last sensor operate status
void printLastOperateStatus(BNO::eStatus_t eStatus)
{
  switch(eStatus) {
  case BNO::eStatusOK:    Serial.println("everything ok"); break;
  case BNO::eStatusErr:   Serial.println("unknow error"); break;
  case BNO::eStatusErrDeviceNotDetect:    Serial.println("device not detected"); break;
  case BNO::eStatusErrDeviceReadyTimeOut: Serial.println("device ready time out"); break;
  case BNO::eStatusErrDeviceStatus:       Serial.println("device internal status error"); break;
  default: Serial.println("unknow status"); break;
  }
}

void setup()
{
  pinMode(4, OUTPUT);
  pinMode(3, OUTPUT);
  Serial.begin(115200);
  bno.reset();
  while(bno.begin() != BNO::eStatusOK) {
    Serial.println("bno begin faild");
    printLastOperateStatus(bno.lastOperateStatus);
    delay(2000);
  }
  Serial.println("bno begin success");
  BNO::sEulAnalog_t   sEul;
  sEul = bno.getEul();
  start_st = float(sEul.head);
  roll_st = float(sEul.roll);
  pitch_st = float(sEul.pitch);


}

void loop()
{
  if(head_st == 0){
    temp_st = start_st;
  }
  BNO::sEulAnalog_t   sEul;
  sEul = bno.getEul();
  head_st = float(sEul.head);
  roll_st = float(sEul.roll);
  pitch_st = float(sEul.pitch);
  Serial.print("yaw:");
  Serial.print(head_st);
  Serial.println(" ");
  Serial.print("temp_st:");
  Serial.print(head_st);
    Serial.println(" ");
  Serial.print("roll:");
  Serial.print(roll_st);
    Serial.println(" ");
  Serial.print("pitch:");
  Serial.print(pitch_st);
    Serial.println(" ");
  
  if(head_st > temp_st+0.3){
    digitalWrite(4, HIGH);
    Serial.print("right");
    delay(100);
    digitalWrite(4, LOW);
   
  }

  if(head_st < temp_st-0.3){
    digitalWrite(3, HIGH);
    Serial.print("left");
    delay(100);
    digitalWrite(3, LOW);
    
  }
  temp_st = head_st;
  delay(100);
}