/*Feedback contgrol of the angle
 * NOTE:MUST PUT THE SKETCH IN A SEPERATE FOLDER, THE SAME NAME AS THE SKETCH!
 * THE SKETCH IS NOT TO BE PUT IN THE SAME FOLDER WITH OTHER SKETCHES
 * THE LIBRARY TO BE USED IS 'hartway_digital'
============================================================
 *   A4988 stepper motor driver (connected to Arduino):
 *      MS1:H, MS2:H, MS3:L, thus 8th-step resolution
 *      MS1: 5V on Arduino (wired to POSITIVE on breadboard)
 *      MS2: 5V on Arduino (wired to POSITIVE on breadboard)
 *      MS3: GND on Arduino (wired to NEGATIVE  on breadboard)
 *      RESET: reset
 *      SLEEP: sleep
 *      STEP: D13
 *      DIR: D12
 *      GND: GND on Arduino (wired to NEGATIVE on breadboard)
 *      VDD: 5V on Arduino (wired to POSITIVE on breadboard)
 *      B1: black wire of stepper motor
 *      A1: green wire of stepper motor
 *      A2: red wire of stepper motor
 *      B2: blue wire of stepper motor
 *      GND (motor supply): GND of motor power supply (wall charger)
 *      VMOT: POSITIVE of motor power supply (wall charger)
 *      
 *    
 *    NEMA-17 17HS15-1684S-PG27 stepper motor (connected to stepper motor driver):
 *      Black wire: B1
 *      Green wire: A1
 *      Red wire: A2
 *      Blue wire: B2
 *      
 *   
 *    ELECTROLYTIC CAPACITOR (>47 uF) (connected to stepper motor driver):
 *      POSITIVE: VMOT
 *      NEGATIVE: GND 
 *      
 *   Accelerometer
 *      //VCC  -  5V
 *      /GND  -  GND
 *      //SDA  -  A4
 *      //SCL  -  A5
 *      //INT - port-2
 *    
 *   
============================================================
Accelerometer
============================================================
   NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
   depends on the MPU-6050's INT pin being connected to the Arduino's
   external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
   digital I/O pin 2.

*/

// Variables for storing and sorting through data received through serial communications
const byte numChars = 100;   // Char array size can be increased if needed.
char receivedChars[numChars];    // Array in which received data will be stored
char tempChars[numChars];        // temporary array for use when parsing
boolean newData = false;


//====================================================================
//Stepper Motor
//====================================================================
//Stepper motor pins

#define dir  12   // "Direction" pin of driver.  Defines direction of motor rotation.
#define pul  13   // 'STEP' pin of driver

//Define variables values
float setPitch = 0, livePitch; //set pitch and measured pitch
float setRoll = 10,liveRoll;//set roll and measured roll 

//====================================================================
//Accelerometer
//====================================================================
// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

MPU6050 mpu;



// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
// pitch/roll angles (in degrees) calculated from the quaternions coming
// from the FIFO. Note this also requires gravity vector calculations.
// Also note that yaw/pitch/roll angles suffer from gimbal lock (for
// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
#define OUTPUT_READABLE_YAWPITCHROLL

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer


// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorFloat gravity;    // [x, y, z]            gravity vector
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

//Interrupt direction routine
volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}  


//==============================================================================================
//                                SETUP
//==============================================================================================

void setup() {
  Serial.begin(115200);
  while (!Serial);

//====================================================================
//Accelerometer
//====================================================================
    // initialize device
    Serial.println(F("Initializing I2C devices..."));
    mpu.initialize();

    // verify connection
    Serial.println(F("Testing device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

    // load and configure the DMP
    Serial.println(F("Initializing DMP..."));
    devStatus = mpu.dmpInitialize();

    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(27);
    mpu.setYGyroOffset(178);
    mpu.setZGyroOffset(-482);
    mpu.setZAccelOffset(1688); // 1688 factory default for my test chip

    // make sure it worked (returns 0 if so)
    if (devStatus == 0) {
        // turn on the DMP, now that it's ready
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
        attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        Serial.println(F("DMP ready! Waiting for first interrupt..."));
        dmpReady = true;

        // get expected DMP packet size for later comparison
        packetSize = mpu.dmpGetFIFOPacketSize();
 
    } else {
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        Serial.print(F("DMP Initialization failed (code "));
        Serial.print(devStatus);
        Serial.println(F(")"));
    }
  
//====================================================================
//Stepper Motor
//====================================================================
  pinMode(dir, OUTPUT);
  pinMode(pul, OUTPUT);
}


void loop() {

  //Check for new set data from serial communication
  if (newData == true) {
    strcpy(tempChars, receivedChars);
    // this temporary copy is necessary to protect the original data
    //   because strtok() used in parseData() replaces the commas with \0
    parseData();    // Sort through data and save to variables
    newData = false;
  }
  
  //Live angel measurement start
  // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO count
    fifoCount = mpu.getFIFOCount();

    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        // reset so we can continue cleanly
        mpu.resetFIFO();
        Serial.println(F("FIFO overflow!"));
    }

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    else if (mpuIntStatus & 0x02) {
      // wait for correct available data length, should be a VERY short wait
      while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

      // read a packet from FIFO
      mpu.getFIFOBytes(fifoBuffer, packetSize);
      mpu.resetFIFO();
        
      // track FIFO count here in case there is > 1 packet available
      // (this lets us immediately read more without waiting for an interrupt)
      fifoCount -= packetSize;


      #ifdef OUTPUT_READABLE_YAWPITCHROLL
        // display Euler angles in degrees
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

        livePitch = ypr[1] * 180/M_PI;
        liveRoll = ypr[2] * 180/M_PI;
       
      #endif
      
    }

 
  
  //Stepper motor rotate till the difference between set and measured angle is less than 0.1deg
  if ((livePitch-setPitch) > 0.1){
      digitalWrite(dir, HIGH);
      digitalWrite(pul, HIGH);
      delayMicroseconds(50);
      digitalWrite(pul, LOW);
      delayMicroseconds(50);
      Serial.println("board counterclockwise");
      print2Serial();
   
  }
  else if ((livePitch-setPitch)<-0.1){
      digitalWrite(dir, LOW);
      digitalWrite(pul, HIGH);
      delayMicroseconds(50);
      digitalWrite(pul, LOW);
      delayMicroseconds(50);
      Serial.println("board clockwise");
      print2Serial();
  }
  else{
      digitalWrite(pul,LOW);
  }

}


void recvWithStartEndMarkers() {
    static boolean recvInProgress = false;
    static byte ndx = 0;
    char startMarker = '<';
    char endMarker = '>';
    char rc;

    while (Serial.available() > 0 && newData == false) {
        rc = Serial.read();

        if (recvInProgress == true) {
            if (rc != endMarker) {
                receivedChars[ndx] = rc;
                ndx++;
                if (ndx >= numChars) {
                    ndx = numChars - 1;
                }
            }
            else {
                receivedChars[ndx] = '\0'; // terminate the string
                recvInProgress = false;
                ndx = 0;
                newData = true;
            }
        }

        else if (rc == startMarker) {
            recvInProgress = true;
        }
    }
}

//============

void parseData() {      // split the data into its parts

    char * strtokIndx; // this is used by strtok() as an index

    // Get and set first float: setTC1
    
    strtokIndx = strtok(tempChars, ",");      // get the first part - the first float (need first part 
                                              // to take in "tempChars", where characters are being stored).
    setPitch = atof(strtokIndx);                // Use "atof" to convert the character array to a float


    // Get and set second float: setTC2
 
    strtokIndx = strtok(NULL, ",");       // this continues where the previous call left off
    setRoll = atof(strtokIndx);
}

//============
void print2Serial(){
  Serial.print("<"); 
  Serial.print(setPitch, 4);
  Serial.print(", ");
  Serial.print(setRoll, 4);
  Serial.print(", ");
  Serial.print(livePitch, 4);
  Serial.print(", ");
  Serial.print(liveRoll, 4);
  Serial.println(">");
  
}