#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
#include <SPI.h>
#include <Mirf.h>
#include <nRF24L01.h>
#include <MirfHardwareSpiDriver.h>
MPU6050 mpu;
#define LED_PIN 13
unsigned int adata = 0;
bool blinkState = false;
bool dmpReady = false;  
uint8_t mpuIntStatus;   
uint8_t devStatus;      
uint16_t packetSize;    
uint16_t fifoCount;     
uint8_t fifoBuffer[64];
//使用卡爾曼濾波計算MPU6050角度  
Quaternion q;           // [w, x, y, z]         
VectorInt16 aa;         // [x, y, z]            
VectorInt16 aaReal;     // [x, y, z]            
VectorInt16 aaWorld;    // [x, y, z]            
VectorFloat gravity;    // [x, y, z]            
float euler[3];         // [psi, theta, phi]    
float ypr[3];           // [yaw, pitch, roll]   
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
volatile bool mpuInterrupt = false;     
void dmpDataReady() {
  mpuInterrupt = true;
}
void setup() {
  //定義腳位
  Mirf.cePin = 7; 
  Mirf.csnPin = 8;
  Mirf.spi = &MirfHardwareSpi;
  Mirf.init();         //初始化nRF24L01  
  Mirf.setRADDR((byte *)"Sen01");   //定義地址
  //定義收發的字節數
  //sizeof(unsigned int) = 2byte
  Mirf.payload = (sizeof(byte)*3);
  //定義通道
  Mirf.channel = 3;
  Mirf.config();
  Wire.begin();
  TWBR = 24;
  Serial.begin(115200);
  while (!Serial);
  Serial.println(F("Initializing I2C devices..."));
  mpu.initialize();
  Serial.println(F("Testing device connections..."));
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
  Serial.println(F("\nSend any character to begin DMP programming and demo: "));
  Serial.println(F("Initializing DMP..."));
    
    devStatus = mpu.dmpInitialize();
    mpu.setXAccelOffset(-2477);
    mpu.setYAccelOffset(-1150);
    mpu.setZAccelOffset(881);
    mpu.setXGyroOffset(51);
    mpu.setYGyroOffset(-7);
    mpu.setZGyroOffset(4);
  if (devStatus == 0) {
    Serial.println(F("Enabling DMP..."));
    mpu.setDMPEnabled(true);
    Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
    attachInterrupt(0, dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();
    Serial.println(F("DMP ready! Waiting for first interrupt..."));
    dmpReady = true;
    packetSize = mpu.dmpGetFIFOPacketSize();
  }
  else {
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
  }
  pinMode(LED_PIN, OUTPUT);
}
void loop() {
  if (!dmpReady) return;
  while (!mpuInterrupt && fifoCount < packetSize) {}
  mpuInterrupt = false;
  mpuIntStatus = mpu.getIntStatus();
  fifoCount = mpu.getFIFOCount();
  if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
      mpu.resetFIFO();
    Serial.println(F("FIFO overflow!"));
  }
  else if (mpuIntStatus & 0x02) {
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
    mpu.getFIFOBytes(fifoBuffer, packetSize);
    fifoCount -= packetSize;
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    byte data[Mirf.payload]; //定義一個數值 大小為Mirf.payload。
    int flex = analogRead(A0);
    byte R = map(ypr[2] * 180/M_PI+90,0,180,0,180);  //將弧度轉為角度
    byte C = map(ypr[1] * 180/M_PI+90,0,180,0,180);
    byte F = map(flex,550,800,110,30);
    //Serial.print('R'); Serial.print(R); Serial.print("\t");
    Serial.print('R'); Serial.print(R); Serial.print("\t");
    Serial.print('C'); Serial.print(C);  Serial.print("\t");
    Serial.print("flex"); Serial.print(flex);  Serial.print("\t");
    Serial.print('F'); Serial.print(F);  Serial.println("\t");
    //Serial.write(data[3]); Serial.write(data[2]); Serial.println("\t");
    //將數值拆解
    data[0] = R;
    data[1] = C;
    data[2] = F;
    //向"Sevo"傳送數值 
    Mirf.send(data); while (Mirf.isSending()) {} //等待傳送完畢
    blinkState = !blinkState;
    digitalWrite(LED_PIN, blinkState);
  }
}